flipped_interface.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 # Imports
00008 ##############################################################################
00009 
00010 import copy
00011 import re
00012 
00013 import rospy
00014 import rosgraph
00015 import rocon_gateway_utils
00016 from gateway_msgs.msg import RemoteRuleWithStatus
00017 
00018 from . import utils
00019 from . import interactive_interface
00020 
00021 ##############################################################################
00022 # Flipped Interface
00023 ##############################################################################
00024 
00025 
00026 class FlippedInterface(interactive_interface.InteractiveInterface):
00027 
00028     '''
00029       The flipped interface is the set of rules
00030       (pubs/subs/services/actions) and rules controlling flips
00031       to other gateways.
00032     '''
00033 
00034     def __init__(self, firewall, default_rule_blacklist, default_rules, all_targets):
00035         '''
00036           Initialises the flipped interface.
00037 
00038           @param firewall : flag to prevent this gateway from accepting flips
00039           @type Bool
00040           @param default_rule_blacklist : used when in flip all mode
00041           @type dictionary of gateway
00042           @param default_rules : static rules to flip on startup
00043           @type gateway_msgs.msg.RemoteRule[]
00044           @param all_targets : static flip all targets to flip to on startup
00045           @type string[]
00046 
00047         '''
00048         interactive_interface.InteractiveInterface.__init__(self, default_rule_blacklist, default_rules, all_targets)
00049 
00050         self.firewall = firewall
00051 
00052         # Function aliases
00053         self.flipped = self.active
00054         self.filtered_flips = []
00055         self.flip_status = utils.create_empty_connection_type_dictionary()
00056         self.flip_all = self.add_all
00057         self.unflip_all = self.remove_all
00058 
00059     ##########################################################################
00060     # Monitoring
00061     ##########################################################################
00062 
00063     def update(self, connections, remote_gateway_hub_index, unique_name, master):
00064         '''
00065           Computes a new flipped interface and returns two dictionaries -
00066           removed and newly added flips so the watcher thread can take
00067           appropriate action (inform the remote gateways).
00068 
00069           This is run in the watcher thread (warning: take care - other
00070           additions come from ros service calls in different threads!)
00071 
00072           @param connections : list of all the system state connections from the local master
00073           @type connection type keyed dictionary of utils.Connection lists.
00074 
00075           @param remote_gateway_hub_index : full gateway-hub database index to parse
00076           @type gateway hash names keyed into a dic with a list of their hubs
00077 
00078           @param unique_name : this gateway's unique hash name
00079           @type string
00080         
00081           @param master : local master
00082           @type rocon_gateway.LocalMaster
00083 
00084           @return new_flips, removed_flips (i.e. those that are no longer on the local master)
00085           @rtype pair of connection type keyed dictionary of gateway_msgs.msg.Rule lists.
00086         '''
00087         # SLOW, EASY METHOD
00088         remote_gateways = remote_gateway_hub_index.keys()
00089 
00090         self._lock.acquire()
00091 
00092         self.flipped = self._prune_unavailable_gateway_flips(self.flipped, remote_gateways) # Prune locally cached flip list for flips that have lost their remotes, keep the rules though
00093 
00094         new_flips, removed_flips, flipped = self._prepare_flips(connections, remote_gateways, unique_name, master)  # Totally regenerate a new flipped interface, compare with old
00095         flip_status = self._prepare_flip_status(flipped)
00096         new_flips, filtered_flips = self._filter_flipped_in_interfaces(new_flips, self.registrations)
00097         self.flipped = self._update_flipped(flipped, filtered_flips)
00098         self._lock.release()
00099         return new_flips, removed_flips
00100 
00101         # OPTIMISED METHOD
00102         #   Keep old rule state and old flip rules/patterns around
00103         #
00104         #   1 - If flip rules/patterns disappeared [diff(old_rules,new_rules)]
00105         #         Check if the current flips are still valid
00106         #         If not all are, remove and unflip them
00107         #
00108         #   2 - If rules disappeared [diff(old_conns,new_conns)]
00109         #         If matching any in flipped, remove and unflip
00110         #
00111         #   3 - If flip rules/patterns appeared [diff(new_rules,old_rules)]
00112         #         parse all conns, if match found, flip
00113         #
00114         #   4 - If rules appeared [diff(new_conns,old_conns)]
00115         #         check for matches, if found, flou
00116         #
00117         # utils.difflist = lambda l1,l2: [x for x in l1 if x not in l2] # diff of lists
00118 
00119     def _update_flipped(self, flipped, filtered_flips):
00120         updated_flipped = {}
00121         for connection_type in flipped.keys():
00122             updated_flipped[connection_type] = [copy.deepcopy(r) for r in flipped[connection_type] if not r in filtered_flips[connection_type]]
00123         return updated_flipped
00124 
00125     def _filter_flipped_in_interfaces(self, new_flips, flipped_in_registrations):
00126         '''
00127           Gateway should not flip out the flipped-in interface.
00128         '''
00129         filtered_flips = utils.create_empty_connection_type_dictionary()
00130         for connection_type in utils.connection_types:
00131             for rule in flipped_in_registrations[connection_type]:
00132                 r = self._is_registration_in_remote_rule(rule, new_flips[connection_type])
00133                 if r:
00134                     filtered_flips[connection_type].append(r)
00135 
00136         for connection_type in new_flips.keys():
00137             new_flips[connection_type] = [r for r in new_flips[connection_type] if not r in filtered_flips[connection_type]]
00138 
00139         rospy.logdebug("Gateway : filtered flip list to prevent cyclic flipping - %s"%str(filtered_flips))
00140 
00141         return new_flips, filtered_flips
00142 
00143     def _is_registration_in_remote_rule(self, rule, new_flip_remote_rules):
00144         for r in new_flip_remote_rules:
00145             node = r.rule.node.split(",")[0]
00146             if rule.local_node == node and rule.remote_gateway == r.gateway and rule.connection.rule.name == r.rule.name:
00147                 return r
00148         return None
00149 
00150     def update_flip_status(self, flip, status):
00151         '''
00152           Update the status of a flip from the hub. This should be called right
00153           after update once self.flipped is established
00154 
00155           @return True if status was indeed changed, False otherwise
00156           @rtype Boolean
00157         '''
00158         state_changed = False
00159         self._lock.acquire()
00160         try:
00161             index = self.flipped[flip.rule.type].index(flip)
00162             state_changed = (self.flip_status[flip.rule.type][index] != status)
00163             self.flip_status[flip.rule.type][index] = status
00164         except ValueError:
00165             pass
00166         self._lock.release()
00167         return state_changed
00168 
00169     def remove_flip(self, flip):
00170         '''
00171           Removes a flip, so that it can be resent as necessary
00172         '''
00173         self._lock.acquire()
00174         try:
00175             self.flipped[flip.rule.type].remove(flip)
00176         except ValueError:
00177             pass
00178         self._lock.release()
00179 
00180     ##########################################################################
00181     # Utility Methods
00182     ##########################################################################
00183 
00184     def _generate_flips(self, connection_type, name, node, remote_gateways, unique_name, master):
00185         '''
00186           Checks if a local rule (obtained from master.get_system_state)
00187           is a suitable association with any of the rules or patterns. This can
00188           return multiple matches, since the same local rule
00189           properties can be multiply flipped to different remote gateways.
00190 
00191           Used in the update() call above that is run in the watcher thread.
00192           Note, don't need to lock here as the update() function takes care of it.
00193 
00194           @param connection_type : rule type
00195           @type str : string constant from gateway_msgs.msg.Rule
00196 
00197           @param name : fully qualified topic, service or action name
00198           @type str
00199 
00200           @param node : ros node name (coming from master.get_system_state)
00201           @type str
00202 
00203           @param gateways : gateways that are available (registered on the hub)
00204           @type string
00205 
00206           @param master : local master
00207           @type rocon_gateway.LocalMaster
00208 
00209           @return all the flip rules that match this local rule
00210           @return list of RemoteRule objects updated with node names from self.watchlist
00211         '''
00212         matched_flip_rules = []
00213         for flip_rule in self.watchlist[connection_type]:
00214             # Check if the flip rule corresponds to an existing gateway
00215             matched_gateways = self._get_matched_gateways(flip_rule, remote_gateways)
00216             if not matched_gateways:
00217                 continue
00218 
00219             # Check names
00220             rule_name = flip_rule.rule.name
00221             matched = self.is_matched(flip_rule, rule_name, name, node)
00222 
00223             if not utils.is_all_pattern(flip_rule.rule.name):
00224                 if not matched:
00225                     rule_name = '/' + unique_name + '/' + flip_rule.rule.name
00226                     matched = self.is_matched(flip_rule, rule_name, name, node)
00227 
00228                 # If it is still not matching
00229                 if not matched:
00230                     rule_name = '/' + flip_rule.rule.name
00231                     matched = self.is_matched(flip_rule, rule_name, name, node)
00232 
00233             if matched:
00234                 for gateway in matched_gateways:
00235                     try:
00236                         matched_flip = copy.deepcopy(flip_rule)
00237                         matched_flip.gateway = gateway  # just in case we used a regex or matched basename
00238                         matched_flip.rule.name = name  # just in case we used a regex
00239                         matched_flip.rule.node = "%s,%s"%(node, master.lookupNode(node)) # just in case we used a regex
00240                         matched_flip_rules.append(matched_flip)
00241                     except rosgraph.masterapi.MasterError as e:
00242                         # Node has been gone already. skips sliently
00243                         pass
00244         return matched_flip_rules
00245 
00246     def _prune_unavailable_gateway_flips(self, flipped, remote_gateways):
00247         # Prune locally cached flip list for flips that have lost their remotes, keep the rules though
00248         for connection_type in utils.connection_types:
00249             # flip.gateway is a hash name, so is the remote_gateways list
00250             flipped[connection_type] = [flip for flip in self.flipped[connection_type] if flip.gateway in remote_gateways]
00251         return flipped
00252 
00253     def _prepare_flips(self, connections, remote_gateways, unique_name, master):
00254         # Variable preparations
00255         flipped         = utils.create_empty_connection_type_dictionary()
00256         new_flips       = utils.create_empty_connection_type_dictionary()
00257         removed_flips   = utils.create_empty_connection_type_dictionary()
00258 
00259         for connection_type in connections:
00260             for connection in connections[connection_type]:
00261                 matched_flip_rules = self._generate_flips(connection.rule.type, connection.rule.name, connection.rule.node, remote_gateways, unique_name, master)
00262                 flipped[connection_type].extend(matched_flip_rules)
00263 
00264             new_flips[connection_type] = utils.difflist(flipped[connection_type], self.flipped[connection_type])
00265             removed_flips[connection_type] = utils.difflist(self.flipped[connection_type], flipped[connection_type])
00266         return new_flips, removed_flips, flipped
00267 
00268     def _prepare_flip_status(self, flipped):
00269         flip_status     = utils.create_empty_connection_type_dictionary()
00270 
00271         # set flip status to unknown first, and then read previous status if available
00272         for connection_type in utils.connection_types:
00273             flip_status[connection_type] = [RemoteRuleWithStatus.UNKNOWN] * len(flipped[connection_type])
00274 
00275         for connection_type in utils.connection_types:
00276             for new_index, flip in enumerate(flipped[connection_type]):
00277                 try:
00278                     index = self.flipped[connection_type].index(flip)
00279                     flip_status[connection_type][new_index] = self.flip_status[connection_type][index]
00280                 except:
00281                     # The new flip probably did not exist. Let it remain unknown
00282                     pass
00283         self.flip_status = copy.deepcopy(flip_status)
00284 
00285         return flip_status
00286 
00287     def _get_matched_gateways(self, flip_rule, remote_gateways):
00288         matched_gateways = []
00289         for gateway in remote_gateways:
00290             # check for regular expression or perfect match
00291             gateway_match_result = re.match(flip_rule.gateway, gateway)
00292             if gateway_match_result and gateway_match_result.group() == gateway:
00293                 matched_gateways.append(gateway)
00294             elif flip_rule.gateway == rocon_gateway_utils.gateway_basename(gateway):
00295                 matched_gateways.append(gateway)
00296         return matched_gateways
00297 
00298 
00299     ##########################################################################
00300     # Accessors for Gateway Info
00301     ##########################################################################
00302     def get_flipped_connections(self):
00303         '''
00304           Gets the flipped connections list for GatewayInfo consumption.
00305 
00306           @return the list of flip rules that are activated and have been flipped.
00307           @rtype RemoteRule[]
00308         '''
00309         flipped_connections = []
00310         for connection_type in utils.connection_types:
00311             for i, connection in enumerate(self.flipped[connection_type]):
00312                 flipped_connections.append(RemoteRuleWithStatus(connection, self.flip_status[connection_type][i]))
00313         return flipped_connections
00314 
00315 
00316 if __name__ == "__main__":
00317 
00318     gateways = ['dude', 'dudette']
00319     dudes = ['fred', 'dude']
00320     dudes[:] = [x for x in dudes if x in gateways]
00321     print dudes


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