gateway.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # License: BSD
00004 #   https://raw.github.com/robotics-in-concert/rocon_multimaster/license/LICENSE
00005 #
00006 
00007 ###############################################################################
00008 # Imports
00009 ###############################################################################
00010 
00011 import copy
00012 import rospy
00013 import gateway_msgs.msg as gateway_msgs
00014 import gateway_msgs.srv as gateway_srvs
00015 
00016 from gateway_msgs.msg import RemoteRuleWithStatus as FlipStatus
00017 
00018 from . import utils
00019 from . import ros_parameters
00020 from .watcher_thread import WatcherThread
00021 from .flipped_interface import FlippedInterface
00022 from .public_interface import PublicInterface
00023 from .pulled_interface import PulledInterface
00024 from .master_api import LocalMaster
00025 from .network_interface_manager import NetworkInterfaceManager
00026 
00027 ###############################################################################
00028 # Thread
00029 ###############################################################################
00030 
00031 
00032 class Gateway(object):
00033 
00034     '''
00035       Used to synchronise with hubs.
00036     '''
00037 
00038     def __init__(self, hub_manager, param, unique_name, publish_gateway_info_callback):
00039         '''
00040         @param hub_manager : container for all the hubs this gateway connects to
00041         @type hub_api.HubManmager
00042 
00043         @param param : parameters set by ros_parameters.py
00044         @type : dictionary of parameter key-value pairs
00045 
00046         @param unique_name : gateway name (param['name']) with unique uuid hash appended
00047 
00048         @param publish_gateway_info_callback : callback for publishing gateway info
00049         '''
00050         self.hub_manager = hub_manager
00051         self.master = LocalMaster()
00052         self.ip = self.master.get_ros_ip()  # gateway is always assumed to sit on the same ip as the master
00053         self._param = param
00054         self._unique_name = unique_name
00055         self._publish_gateway_info = publish_gateway_info_callback
00056         default_rule_blacklist = ros_parameters.generate_rules(self._param["default_blacklist"])
00057         default_rules, all_targets = ros_parameters.generate_remote_rules(self._param["default_flips"])
00058         self.flipped_interface = FlippedInterface(
00059             firewall=self._param['firewall'],
00060             default_rule_blacklist=default_rule_blacklist,
00061             default_rules=default_rules,
00062             all_targets=all_targets)
00063         default_rules, all_targets = ros_parameters.generate_remote_rules(self._param["default_pulls"])
00064         self.pulled_interface = PulledInterface(default_rule_blacklist=default_rule_blacklist,
00065                                                 default_rules=default_rules,
00066                                                 all_targets=all_targets)
00067         self.public_interface = PublicInterface(
00068             default_rule_blacklist=default_rule_blacklist,
00069             default_rules=ros_parameters.generate_rules(self._param['default_advertisements']))
00070         if self._param['advertise_all']:
00071             # no extra blacklist beyond the default (keeping it simple in yaml for now)
00072             self.public_interface.advertise_all([])
00073 
00074         self.network_interface_manager = NetworkInterfaceManager(self._param['network_interface'])
00075         self.watcher_thread = WatcherThread(self, self._param['watch_loop_period'])
00076 
00077     def spin(self):
00078         self.watcher_thread.start()
00079 
00080     def shutdown(self):
00081         for connection_type in utils.connection_types:
00082             for flip in self.flipped_interface.flipped[connection_type]:
00083                 self.hub_manager.send_unflip_request(flip.gateway, flip.rule)
00084             for registration in self.flipped_interface.registrations[connection_type]:
00085                 self.master.unregister(registration)
00086             for registration in self.pulled_interface.registrations[connection_type]:
00087                 self.master.unregister(registration)
00088 
00089     def is_connected(self):
00090         '''
00091           We often check if we're connected to any hubs often just to ensure we
00092           don't waste time processing if there is no-one listening.
00093 
00094           @return True if at least one hub is connected, False otherwise
00095           @rtype Bool
00096         '''
00097         return self.hub_manager.is_connected()
00098 
00099     def disengage_hub(self, hub):
00100         '''
00101           Disengage from the specified hub. Don't actually need to clean up connections
00102           here like we do in shutdown - that can be handled from the watcher thread itself.
00103 
00104           @param hub : the hub that will be deleted.
00105         '''
00106         self.hub_manager.disengage_hub(hub)
00107         self._publish_gateway_info()
00108 
00109     ##########################################################################
00110     # Update interface states (jobs assigned from watcher thread)
00111     ##########################################################################
00112 
00113     def update_flipped_interface(self, local_connection_index, remote_gateway_hub_index):
00114         '''
00115           Process the list of local connections and check against
00116           the current flip rules and patterns for changes. If a rule
00117           has become (un)available take appropriate action.
00118 
00119           @param local_connection_index : list of current local connections parsed from the master
00120           @type : dictionary of ConnectionType.xxx keyed lists of utils.Connections
00121 
00122           @param gateways : list of remote gateway string id's
00123           @type string
00124         '''
00125         state_changed = False
00126 
00127         # Get flip status of existing requests, and remove those requests that need to be resent
00128         flipped_connections = self.flipped_interface.get_flipped_connections()
00129         for flip in flipped_connections:
00130             if flip.remote_rule.gateway in remote_gateway_hub_index:
00131                 for hub in remote_gateway_hub_index[flip.remote_rule.gateway]:
00132                     status = hub.get_flip_request_status(flip.remote_rule)
00133                     if status == FlipStatus.RESEND:
00134                         rospy.loginfo("Gateway : resend requested for flip request [%s]%s" %
00135                                       (flip.remote_rule.gateway, utils.format_rule(flip.remote_rule.rule)))
00136                         # Remove the flip, so that it will be resent as part of new_flips
00137                         self.flipped_interface.remove_flip(flip.remote_rule)
00138                         hub.send_unflip_request(flip.remote_rule.gateway, flip.remote_rule.rule)
00139                         hub.remove_flip_details(flip.remote_rule.gateway,
00140                                                 flip.remote_rule.rule.name,
00141                                                 flip.remote_rule.rule.type,
00142                                                 flip.remote_rule.rule.node)
00143                         break
00144 
00145         new_flips, lost_flips = self.flipped_interface.update(
00146             local_connection_index, remote_gateway_hub_index, self._unique_name, self.master)
00147         for connection_type in utils.connection_types:
00148             for flip in new_flips[connection_type]:
00149                 firewall_flag = self.hub_manager.get_remote_gateway_firewall_flag(flip.gateway)
00150                 if firewall_flag:
00151                     continue
00152                 state_changed = True
00153                 # for actions, need to post flip details here
00154                 connections = self.master.generate_connection_details(flip.rule.type, flip.rule.name, flip.rule.node)
00155                 if (connection_type == utils.ConnectionType.ACTION_CLIENT or
00156                         connection_type == utils.ConnectionType.ACTION_SERVER):
00157                     rospy.loginfo("Gateway : sending flip request [%s]%s" %
00158                                   (flip.gateway, utils.format_rule(flip.rule)))
00159                     hub = remote_gateway_hub_index[flip.gateway][0]
00160                     hub.post_flip_details(flip.gateway, flip.rule.name, flip.rule.type, flip.rule.node)
00161                     for connection in connections:
00162                         hub.send_flip_request(flip.gateway, connection)  # flip the individual pubs/subs
00163                 else:
00164                     for connection in connections:
00165                         rospy.loginfo("Gateway : sending flip request [%s]%s" %
00166                                       (flip.gateway, utils.format_rule(flip.rule)))
00167                         hub = remote_gateway_hub_index[flip.gateway][0]
00168                         hub.send_flip_request(flip.gateway, connection)
00169                         hub.post_flip_details(
00170                             flip.gateway, flip.rule.name, flip.rule.type, flip.rule.node)
00171             for flip in lost_flips[connection_type]:
00172                 state_changed = True
00173                 rospy.loginfo("Gateway : sending unflip request [%s]%s" % (flip.gateway, utils.format_rule(flip.rule)))
00174                 for hub in remote_gateway_hub_index[flip.gateway]:
00175                     rule = copy.deepcopy(flip.rule)
00176                     rule.node = rule.node.split(",")[0] # strip out xmlrpc uri to send unflip request
00177                     if hub.send_unflip_request(flip.gateway, rule):
00178                         # This hub was used to send the original flip request
00179                         hub.remove_flip_details(flip.gateway, flip.rule.name, flip.rule.type, flip.rule.node)
00180                         break
00181 
00182         # Update flip status
00183         flipped_connections = self.flipped_interface.get_flipped_connections()
00184         for flip in flipped_connections:
00185             for hub in remote_gateway_hub_index[flip.remote_rule.gateway]:
00186                 rule = copy.deepcopy(flip.remote_rule)
00187                 rule.rule.node = rule.rule.node.split(",")[0]
00188                 status = hub.get_flip_request_status(flip.remote_rule)
00189                 if status is not None:
00190                     flip_state_changed = self.flipped_interface.update_flip_status(flip.remote_rule, status)
00191                     state_changed = state_changed or flip_state_changed
00192                     break
00193 
00194         if state_changed:
00195             self._publish_gateway_info()
00196 
00197     def update_pulled_interface(self, unused_connections, remote_gateway_hub_index):
00198         '''
00199           Process the list of local connections and check against
00200           the current pull rules and patterns for changes. If a rule
00201           has become (un)available take appropriate action.
00202 
00203           This is called by the watcher thread. The remote_gateway_hub_index
00204           is always a full dictionary of all remote gateway and hub key-value
00205           pairs - it is only included as an argument here to save
00206           processing doubly in the watcher thread.
00207 
00208           @param connections : list of current local connections parsed from the master
00209           @type : dictionary of ConnectionType.xxx keyed lists of utils.Connections
00210 
00211           @param remote_gateway_hub_index : key-value remote gateway name-hub list pairs
00212           @type dictionary of remote_gateway_name-list of hub_api.Hub objects key-value pairs
00213         '''
00214         state_changed = False
00215         remote_connections = {}
00216         for remote_gateway in remote_gateway_hub_index.keys() + self.pulled_interface.list_remote_gateway_names():
00217             remote_connections[remote_gateway] = {}
00218             try:
00219                 for hub in remote_gateway_hub_index[remote_gateway]:
00220                     remote_connections[remote_gateway].update(hub.get_remote_connection_state(remote_gateway))
00221             except KeyError:
00222                 pass  # remote gateway no longer exists on the hub network
00223         new_pulls, lost_pulls = self.pulled_interface.update(remote_connections, self._unique_name)
00224         for connection_type in utils.connection_types:
00225             for pull in new_pulls[connection_type]:
00226                 # dig out the corresponding connection (bit inefficient plouging back into this again
00227                 connection = None
00228                 for remote_gateway in remote_connections.keys():
00229                     for c in remote_connections[remote_gateway][pull.rule.type]:
00230                         if c.rule.name == pull.rule.name and \
00231                            c.rule.node == pull.rule.node:
00232                             connection = c
00233                             break
00234                     if connection:
00235                         break
00236                 # Register this pull
00237                 existing_registration = self.pulled_interface.find_registration_match(
00238                     pull.gateway, pull.rule.name, pull.rule.node, pull.rule.type)
00239                 if not existing_registration:
00240                     rospy.loginfo("Gateway : pulling in connection %s[%s]" %
00241                                   (utils.format_rule(pull.rule), remote_gateway))
00242                     registration = utils.Registration(connection, pull.gateway)
00243                     new_registration = self.master.register(registration)
00244                     if new_registration is not None:
00245                         self.pulled_interface.registrations[registration.connection.rule.type].append(new_registration)
00246                         hub = remote_gateway_hub_index[pull.gateway][0]
00247                         hub.post_pull_details(pull.gateway, pull.rule.name, pull.rule.type, pull.rule.node)
00248                         state_changed = True
00249             for pull in lost_pulls[connection_type]:
00250                 # Unregister this pull
00251                 existing_registration = self.pulled_interface.find_registration_match(
00252                     pull.gateway, pull.rule.name, pull.rule.node, pull.rule.type)
00253                 if existing_registration:
00254                     rospy.loginfo("Gateway : abandoning pulled connection %s[%s]" % (
00255                         utils.format_rule(pull.rule), pull.gateway))
00256                     self.master.unregister(existing_registration)
00257                     # This code was here, but causing bugs...actually it should never remove details from the hub,
00258                     # that is the responsibility of the advertising gateway. TODO confirm this.
00259                     #hub = remote_gateway_hub_index[pull.gateway][0]
00260                     # if hub:
00261                     #    hub.remove_pull_details(pull.gateway, pull.rule.name, pull.rule.type, pull.rule.node)
00262                     self.pulled_interface.registrations[
00263                         existing_registration.connection.rule.type].remove(existing_registration)
00264                     state_changed = True
00265         if state_changed:
00266             self._publish_gateway_info()
00267 
00268     def update_public_interface(self, local_connection_index):
00269         '''
00270           Process the list of local connections and check against
00271           the current rules and patterns for changes. If a rule
00272           has become (un)available take appropriate action.
00273 
00274           @param local_connection_index : list of current local connections parsed from the master
00275           @type : { utils.ConnectionType.xxx : utils.Connection[] } dictionaries
00276         '''
00277         state_changed = False
00278         # new_conns, lost_conns are of type { utils.ConnectionType.xxx : utils.Connection[] }
00279         new_conns, lost_conns = self.public_interface.update(
00280             local_connection_index, self.master.generate_advertisement_connection_details)
00281         # public_interface is of type gateway_msgs.Rule[]
00282         public_interface = self.public_interface.getInterface()
00283         for connection_type in utils.connection_types:
00284             for new_connection in new_conns[connection_type]:
00285                 rospy.loginfo("Gateway : adding connection to public interface %s" %
00286                               utils.format_rule(new_connection.rule))
00287                 self.hub_manager.advertise(new_connection)
00288                 state_changed = True
00289             for lost_connection in lost_conns[connection_type]:
00290                 rospy.loginfo("Gateway : removing connection from public interface %s" %
00291                               utils.format_rule(lost_connection.rule))
00292                 self.hub_manager.unadvertise(lost_connection)
00293                 state_changed = True
00294         if state_changed:
00295             self._publish_gateway_info()
00296         return public_interface
00297 
00298     def update_flipped_in_interface(self, registrations, remote_gateway_hub_index):
00299         '''
00300           Match the flipped in connections to supplied registrations using
00301           supplied registrations, flipping and unflipping as necessary.
00302 
00303           @param registrations : registrations (with status) to be processed
00304           @type list of (utils.Registration, str) where the str contains the status
00305         '''
00306 
00307         hubs = {}
00308         for gateway in remote_gateway_hub_index:
00309             for hub in remote_gateway_hub_index[gateway]:
00310                 hubs[hub.uri] = hub
00311 
00312         update_flip_status = {}
00313         if self.flipped_interface.firewall:
00314             if len(registrations) != 0:
00315                 rospy.logwarn("Gateway : firewalled, but received flip requests...")
00316                 for (registration, status) in registrations:
00317                     for hub in remote_gateway_hub_index[registration.remote_gateway]:
00318                         if hub.uri not in update_flip_status:
00319                             update_flip_status[hub.uri] = []
00320                         update_flip_status[hub.uri].append((registration, FlipStatus.BLOCKED))
00321 
00322             # Mark all these registrations as blocked
00323             for hub_uri, hub in hubs.iteritems():
00324                 if hub_uri in update_flip_status:
00325                     hub.update_multiple_flip_request_status(update_flip_status[hub_uri])
00326             return
00327 
00328         state_changed = False
00329 
00330         # Add new registrations
00331         for (registration, status) in registrations:
00332             # probably not necessary as the flipping gateway will already check this
00333             existing_registration = self.flipped_interface.find_registration_match(
00334                 registration.remote_gateway,
00335                 registration.connection.rule.name,
00336                 registration.connection.rule.node,
00337                 registration.connection.rule.type)
00338             if not existing_registration:
00339                 rospy.loginfo("Gateway : received a flip request %s" % str(registration))
00340                 state_changed = True
00341                 new_registration = self.master.register(registration)
00342                 if new_registration is not None:
00343                     self.flipped_interface.registrations[registration.connection.rule.type].append(new_registration)
00344                 # Update this flip's status
00345                 if status != FlipStatus.ACCEPTED:
00346                     for hub in remote_gateway_hub_index[registration.remote_gateway]:
00347                         if hub.uri not in update_flip_status:
00348                             update_flip_status[hub.uri] = []
00349                         update_flip_status[hub.uri].append((registration, FlipStatus.ACCEPTED))
00350             else:
00351                 # Just make sure that this flip request is marked as accepted
00352                 if status != FlipStatus.ACCEPTED:
00353                     for hub in remote_gateway_hub_index[registration.remote_gateway]:
00354                         if hub.uri not in update_flip_status:
00355                             update_flip_status[hub.uri] = []
00356                         update_flip_status[hub.uri].append((registration, FlipStatus.ACCEPTED))
00357 
00358         # Update the flip status for newly added registrations
00359         for hub_uri, hub in hubs.iteritems():
00360             if hub_uri in update_flip_status:
00361                 hub.update_multiple_flip_request_status(update_flip_status[hub_uri])
00362 
00363         # Remove local registrations that are no longer flipped to this gateway
00364         local_registrations = copy.deepcopy(self.flipped_interface.registrations)
00365         for connection_type in utils.connection_types:
00366             for local_registration in local_registrations[connection_type]:
00367                 matched_registration = None
00368                 for (registration, status) in registrations:
00369                     if registration.connection == local_registration.connection and \
00370                        registration.remote_gateway == local_registration.remote_gateway:
00371                         matched_registration = registration
00372                         break
00373                     else:
00374                         continue
00375                 if matched_registration is None:
00376                     state_changed = True
00377                     rospy.loginfo("Gateway : unflipping received flip %s" % str(local_registration))
00378                     self.master.unregister(local_registration)
00379                     self.flipped_interface.registrations[connection_type].remove(local_registration)
00380 
00381         if state_changed:
00382             self._publish_gateway_info()
00383 
00384     def update_network_information(self):
00385         '''
00386           If we are running over a wired connection, then do nothing.
00387           If over the wireless, updated data transfer rate and signal strength
00388           for this gateway on the hub
00389         '''
00390         statistics = self.network_interface_manager.get_statistics()
00391         self.hub_manager.publish_network_statistics(statistics)
00392 
00393     ##########################################################################
00394     # Incoming commands from local system (ros service callbacks)
00395     ##########################################################################
00396 
00397     def ros_service_set_watcher_period(self, request):
00398         '''
00399           Configures the watcher period. This is useful to slow/speed up the
00400           watcher loop. Quite often you want it polling quickly early while
00401           configuring connections, but on long loops later when it does not have
00402           to do very much except look for shutdown.
00403 
00404           @param request
00405           @type gateway_srvs.SetWatcherPeriodRequest
00406           @return service response
00407           @rtgateway_srvs.srv.SetWatcherPeriodResponse
00408         '''
00409         self.watcher_thread.set_watch_loop_period(request.period)
00410         return gateway_srvs.SetWatcherPeriodResponse(self.watcher_thread.get_watch_loop_period())
00411 
00412     def ros_subscriber_force_update(self, data):
00413         '''
00414           Trigger a watcher loop update
00415         '''
00416         self.watcher_thread.trigger_update = True
00417 
00418     def ros_service_advertise(self, request):
00419         '''
00420           Puts/Removes a number of rules on the public interface watchlist.
00421           As local rules matching these rules become available/go away,
00422           the public interface is modified accordingly. A manual update is done
00423           at the end of the advertise call to quickly capture existing
00424           rules
00425 
00426           @param request
00427           @type gateway_srvs.AdvertiseRequest
00428           @return service response
00429           @rtgateway_srvs.srv.AdvertiseReponse
00430         '''
00431         response = gateway_srvs.AdvertiseResponse()
00432         try:
00433             if not request.cancel:
00434                 for rule in request.rules:
00435                     if not self.public_interface.add_rule(rule):
00436                         response.result = gateway_msgs.ErrorCodes.ADVERTISEMENT_EXISTS
00437                         response.error_message = "advertisment rule already exists [%s:(%s,%s)]" % (
00438                             rule.name, rule.type, rule.node)
00439             else:
00440                 for rule in request.rules:
00441                     if not self.public_interface.remove_rule(rule):
00442                         response.result = gateway_msgs.ErrorCodes.ADVERTISEMENT_NOT_FOUND
00443                         response.error_message = "advertisment not found [%s:(%s,%s)]" % (
00444                             rule.name, rule.type, rule.node)
00445         except Exception as e:
00446             rospy.logerr("Gateway : unknown advertise error [%s]." % str(e))
00447             response.result = gateway_msgs.ErrorCodes.UNKNOWN_ADVERTISEMENT_ERROR
00448 
00449         # Let the watcher get on with the update asap
00450         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00451             self.watcher_thread.trigger_update = True
00452             self._publish_gateway_info()
00453         else:
00454             rospy.logerr("Gateway : %s." % response.error_message)
00455         response.watchlist = self.public_interface.getWatchlist()
00456         return response
00457 
00458     def ros_service_advertise_all(self, request):
00459         '''
00460           Toggles the advertise all mode. If advertising all, an additional
00461           blacklist parameter can be supplied which includes all the topics that
00462           will not be advertised/watched for. This blacklist is added to the
00463           default blacklist of the public interface
00464 
00465           @param request
00466           @type gateway_srvs.AdvertiseAllRequest
00467           @return service response
00468           @rtype gateway_srvs.AdvertiseAllReponse
00469         '''
00470         response = gateway_srvs.AdvertiseAllResponse()
00471         try:
00472             if not request.cancel:
00473                 if not self.public_interface.advertise_all(request.blacklist):
00474                     response.result = gateway_msgs.ErrorCodes.ADVERTISEMENT_EXISTS
00475                     response.error_message = "already advertising all."
00476             else:
00477                 self.public_interface.unadvertise_all()
00478         except Exception as e:
00479             response.result = gateway_msgs.ErrorCodes.UNKNOWN_ADVERTISEMENT_ERROR
00480             response.error_message = "unknown advertise all error [%s]" % (str(e))
00481 
00482         # Let the watcher get on with the update asap
00483         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00484             self.watcher_thread.trigger_update = True
00485             self._publish_gateway_info()
00486         else:
00487             rospy.logerr("Gateway : %s." % response.error_message)
00488         response.blacklist = self.public_interface.getBlacklist()
00489         return response
00490 
00491     def ros_service_flip(self, request):
00492         '''
00493           Puts flip rules on a watchlist which (un)flips them when they
00494           become (un)available.
00495 
00496           @param request
00497           @type gateway_srvs.RemoteRequest
00498           @return service response
00499           @rtype gateway_srvs.RemoteResponse
00500         '''
00501         # could move this below and if any are fails, just abort adding the rules.
00502         # Check if the target remote gateway is valid.
00503         response = self._check_remote_gateways(request.remotes)
00504         if response:
00505             return response
00506 
00507         response = gateway_srvs.RemoteResponse(gateway_msgs.ErrorCodes.SUCCESS, "")
00508 
00509         # result is currently SUCCESS
00510         # Process all add/remove flip requests
00511         if not request.cancel: # Rule add request
00512             response = self._add_flip_rules(request.remotes)
00513         else: # Rule remove request
00514             response = self._remove_flip_rules(request.remotes)
00515 
00516         # Post processing
00517         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00518             self._publish_gateway_info()
00519             self.watcher_thread.trigger_update = True
00520         else:
00521             rospy.logerr("Gateway : %s." % response.error_message)
00522         return response
00523 
00524     def ros_service_flip_all(self, request):
00525         '''
00526           Flips everything except a specified blacklist to a particular gateway,
00527           or if the cancel flag is set, clears all flips to that gateway.
00528 
00529           @param request
00530           @type gateway_srvs.RemoteAllRequest
00531           @return service response
00532           @rtype gateway_srvs.RemoteAllResponse
00533         '''
00534         response = gateway_srvs.RemoteAllResponse()
00535         remote_gateway_target_hash_name, response.result, response.error_message = self._ros_service_remote_checks(
00536             request.gateway)
00537         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00538             if not request.cancel:
00539                 if self.flipped_interface.flip_all(remote_gateway_target_hash_name, request.blacklist):
00540                     rospy.loginfo("Gateway : flipping all to gateway '%s'" % (remote_gateway_target_hash_name))
00541                 else:
00542                     response.result = gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS
00543                     response.error_message = "already flipping all to gateway '%s' " + remote_gateway_target_hash_name
00544             else:  # request.cancel
00545                 self.flipped_interface.unflip_all(remote_gateway_target_hash_name)
00546                 rospy.loginfo("Gateway : cancelling a previous flip all request [%s]" % (request.gateway))
00547         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00548             self._publish_gateway_info()
00549             self.watcher_thread.trigger_update = True
00550         else:
00551             rospy.logerr("Gateway : %s." % response.error_message)
00552         return response
00553 
00554     def ros_service_pull(self, request):
00555         '''
00556           Puts a single rule on a watchlist and pulls it from a particular
00557           gateway when it becomes (un)available.
00558 
00559           @param request
00560           @type gateway_srvs.RemoteRequest
00561           @return service response
00562           @rtype gateway_srvs.RemoteResponse
00563         '''
00564         # could move this below and if any are fails, just abort adding the rules.
00565         # Check if the target remote gateway is valid.
00566         response = self._check_remote_gateways(request.remotes)
00567         if response:
00568             return response
00569 
00570         response = gateway_srvs.RemoteResponse(gateway_msgs.ErrorCodes.SUCCESS, "")
00571 
00572         # result is currently SUCCESS
00573         added_rules = []
00574         for remote in request.remotes:
00575             if not request.cancel:
00576                 pull_rule = self.pulled_interface.add_rule(remote)
00577                 if pull_rule:
00578                     added_rules.append(pull_rule)
00579                     rospy.loginfo("Gateway : added pull rule [%s:(%s,%s)]" %
00580                                   (pull_rule.gateway, pull_rule.rule.name, pull_rule.rule.type))
00581                 else:
00582                     response.result = gateway_msgs.ErrorCodes.PULL_RULE_ALREADY_EXISTS
00583                     response.error_message = "pull rule already exists [%s:(%s,%s)]" % (
00584                         remote.gateway, remote.rule.name, remote.rule.type)
00585                     break
00586             else:  # request.cancel
00587                 for remote in request.remotes:
00588                     removed_pull_rules = self.pulled_interface.remove_rule(remote)
00589                     if removed_pull_rules:
00590                         rospy.loginfo("Gateway : removed pull rule [%s:%s]" % (remote.gateway, remote.rule.name))
00591         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00592             self._publish_gateway_info()
00593             self.watcher_thread.trigger_update = True
00594         else:
00595             if added_rules:  # completely abort any added rules
00596                 for added_rule in added_rules:
00597                     self.pulled_interface.remove_rule(added_rule)
00598             rospy.logerr("Gateway : %s." % response.error_message)
00599         return response
00600 
00601     def ros_service_pull_all(self, request):
00602         '''
00603           Pull everything except a specified blacklist from a particular gateway,
00604           or if the cancel flag is set, clears all pulls from that gateway.
00605 
00606           @param request
00607           @type gateway_srvs.RemoteAllRequest
00608           @return service response
00609           @rtype gateway_srvs.RemoteAllResponse
00610         '''
00611         response = gateway_srvs.RemoteAllResponse()
00612         remote_gateway_target_hash_name, response.result, response.error_message = self._ros_service_remote_checks(
00613             request.gateway)
00614         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00615             if not request.cancel:
00616                 if self.pulled_interface.pull_all(remote_gateway_target_hash_name, request.blacklist):
00617                     rospy.loginfo("Gateway : pulling all from gateway '%s'" % (request.gateway))
00618                 else:
00619                     response.result = gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS
00620                     response.error_message = "already pulling all from gateway '%s' " + request.gateway
00621             else:  # request.cancel
00622                 self.pulled_interface.unpull_all(remote_gateway_target_hash_name)
00623                 rospy.loginfo("Gateway : cancelling a previous pull all request [%s]" % (request.gateway))
00624         if response.result == gateway_msgs.ErrorCodes.SUCCESS:
00625             self._publish_gateway_info()
00626             self.watcher_thread.trigger_update = True
00627         else:
00628             rospy.logerr("Gateway : %s." % response.error_message)
00629         return response
00630 
00631     def _ros_service_remote_checks(self, gateway):
00632         '''
00633           Some simple checks when pulling or flipping to make sure that the remote gateway is visible. It
00634           does a strict check on the hash names first, then falls back to looking for weak matches on the
00635           human friendly name.
00636 
00637           @param gateway : remote gateway target name (can be hash name, basename or regex pattern)
00638           @type string
00639           @return pair of result type and message
00640           @rtype gateway_msgs.ErrorCodes.xxx, string
00641         '''
00642         if not self.is_connected():
00643             return None, gateway_msgs.ErrorCodes.NO_HUB_CONNECTION, "not connected to hub, aborting"
00644         if gateway == self._unique_name:
00645             return None, gateway_msgs.ErrorCodes.REMOTE_GATEWAY_SELF_IS_NOT, "gateway cannot flip/pull to itself"
00646         return gateway, gateway_msgs.ErrorCodes.SUCCESS, ""
00647 #        matches, weak_matches = self.hub_manager.match_remote_gateway_name(gateway)
00648 #        if len(matches) > 1:
00649 #            return None, gateway_msgs.ErrorCodes.REMOTE_GATEWAY_TARGET_HAS_MULTIPLE_MATCHES, \
00650 #                "remote gateway target has multiple matches, invalid [%s][%s]" % (gateway, matches)
00651 #        elif len(matches) == 1:
00652 #            return matches[0], gateway_msgs.ErrorCodes.SUCCESS, ""
00653 # Fallback to checking for weak matches
00654 #        if len(weak_matches) > 1:
00655 #            return None, gateway_msgs.ErrorCodes.REMOTE_GATEWAY_TARGET_HAS_MULTIPLE_MATCHES, \
00656 #                "remote gateway target has multiple matches against hashed names, invalid [%s]" % weak_matches
00657 #        elif len(weak_matches) == 1:
00658 #            return weak_matches[0], gateway_msgs.ErrorCodes.SUCCESS, ""
00659 # Not visible
00660 # return None, gateway_msgs.ErrorCodes.REMOTE_GATEWAY_NOT_VISIBLE, "remote
00661 # gateway is currently not visible on the hubs [%s]" % gateway
00662 
00663     def _check_remote_gateways(self, remotes):
00664         """
00665           Check given gateways in remote rules are valid
00666 
00667           :param remotes: remote rules
00668           :type remotes: gateway_msgs.RemoteRule[]
00669 
00670           :return: whether it is valid, error message if it failes
00671           :rtypes: None or gateway_srvs.RemoteResponse
00672         """
00673         response = gateway_srvs.RemoteResponse()
00674         for remote in remotes:
00675             remote.gateway, response.result, response.error_message = self._ros_service_remote_checks(remote.gateway)
00676             if response.result != gateway_msgs.ErrorCodes.SUCCESS:
00677                 rospy.logerr("Gateway : %s." % response.error_message)
00678                 return response
00679         return None
00680 
00681     def _add_flip_rules(self, remotes):
00682         """
00683           Add given rules into watcher list
00684 
00685           :param remotes: remote rules
00686           :type remotes: gateway_msgs.RemoteRule[]
00687           :return: whether it is successful
00688           :rtypes: gateway_srvs.RemoteResponse
00689         """
00690         response = gateway_srvs.RemoteResponse()
00691         response.result = gateway_msgs.ErrorCodes.SUCCESS
00692 
00693         added_rules = []
00694         for remote in remotes:
00695             flip_rule = self.flipped_interface.add_rule(remote)
00696             if flip_rule:
00697                 added_rules.append(flip_rule)
00698                 rospy.loginfo("Gateway : added flip rule [%s:(%s,%s)]" % (flip_rule.gateway, flip_rule.rule.name, flip_rule.rule.type))
00699             else:
00700                 response.result = gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS
00701                 response.error_message = "flip rule already exists [%s:(%s,%s)]" % (remote.gateway, remote.rule.name, remote.rule.type)
00702 
00703         if response.result != gateway_msgs.ErrorCodes.SUCCESS:
00704             # completely abort any added rules
00705             for added_rule in added_rules:
00706                 self.flipped_interface.remove_rule(added_rule)
00707         return response
00708 
00709     def _remove_flip_rules(self, remotes):
00710         """
00711           remove given rules into watcher list
00712 
00713           :param remotes: remote rules
00714           :type remotes: gateway_msgs.RemoteRule[]
00715           :return: whether it is successful
00716           :rtypes: gateway_srvs.RemoteResponse
00717         """
00718         response = gateway_srvs.RemoteResponse()
00719         response.result = gateway_msgs.ErrorCodes.SUCCESS
00720 
00721         for remote in remotes:
00722             removed_flip_rules = self.flipped_interface.remove_rule(remote)
00723             if removed_flip_rules:
00724                 rospy.loginfo("Gateway : removed flip rule [%s:(%s,%s)]" % (remote.gateway, remote.rule.name, remote.rule.type))
00725         return response


rocon_gateway
Author(s): Daniel Stonier , Jihoon Lee , Piyush Khandelwal
autogenerated on Sat Jun 8 2019 18:48:44