def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph): if node in rosgraphinst.bad_nodes: bn = rosgraphinst.bad_nodes[node] if bn.type == rosgraph.impl.graph.BadNode.DEAD: dotcode_factory.add_node_to_graph(dotgraph, nodename=node, shape="doublecircle", url=node, color="red") else: dotcode_factory.add_node_to_graph(dotgraph, nodename=node, shape="doublecircle", url=node, color="orange") else: dotcode_factory.add_node_to_graph( dotgraph, nodename=node, #nodename=rocon_utilities.gateway_basename(node), #nodelabel=rocon_utilities.gateway_basename(node), shape='ellipse', url=rocon_utilities.gateway_basename(node), #url=node )
def test_graph(self): flips = None while not flips: printtest("Waiting for flips") self.graph.update() flips = self.graph._local_gateway.flip_watchlist rospy.sleep(0.2) printtest( "********************************************************************" ) printtest("* Local Gateway") printtest( "********************************************************************" ) printtest("%s" % self.graph._local_gateway) self.assertEquals("1", str(len(flips))) # TODO: this is currently returning the base name, is should be returning the hash name self.assertEquals("remote_gateway", flips[0].gateway) self.assertEquals("publisher", flips[0].rule.type) self.assertEquals("/chatter", flips[0].rule.name) printtest( "********************************************************************" ) printtest("* Remote Gateway") printtest( "********************************************************************" ) printtest("%s" % self.graph._remote_gateways) for remote_gateway in self.graph._remote_gateways: self.assertEquals( "remote_gateway", rocon_utilities.gateway_basename(remote_gateway.name))
def wait_for_remote_gateway(remote_gateway_name, ns=_gateway_namespace, timeout=rospy.Duration(5.0)): ''' Slowly loop (and block) until remote the gateway is visible on our hub. ''' rospy.wait_for_service(ns + '/remote_gateway_info') remote_gateway_info_service = rospy.ServiceProxy( ns + '/remote_gateway_info', gateway_srvs.RemoteGatewayInfo) req = gateway_srvs.RemoteGatewayInfoRequest() start_time = rospy.Time.now() while not rospy.is_shutdown(): req = gateway_srvs.RemoteGatewayInfoRequest() req.gateways = [] resp = remote_gateway_info_service(req) matched = False if rospy.Time.now() - start_time > timeout: raise GatewaySampleRuntimeError( "timed out waiting for a remote gateway to appear") for gateway in resp.gateways: if remote_gateway_name == gateway.name: matched = True break remote_gateway_basename = rocon_utilities.gateway_basename( gateway.name) print("Samples: gateway comparison [%s][%s]" % (remote_gateway_basename, remote_gateway_name)) if remote_gateway_name == remote_gateway_basename: matched = True break if matched: break
def _generate_pulls(self, connection_type, name, node, gateway, unique_name): ''' Checks if a local rule (obtained from master.get_system_state) is a suitable association with any of the rules or patterns. This can return multiple matches, since the same local rule properties can be multiply pulled to different remote gateways. Used in the update() call above that is run in the watcher thread. Note, don't need to lock here as the update() function takes care of it. @param connection_type : rule type @type str : string constant from gateway_msgs.Rule @param name : fully qualified topic, service or action name @type str @param node : ros node name (coming from master.get_system_state) @type str @param gateway : remote gateway hash name. @type str @return all the pull rules that match this local rule @return list of RemoteRule objects updated with node names from self.watchlist ''' matched_pull_rules = [] for rule in self.watchlist[connection_type]: # check for regular expression or perfect match gateway_match_result = re.match(rule.gateway, gateway) matched = False if gateway_match_result and gateway_match_result.group() == gateway: matched = True elif rule.gateway == rocon_utilities.gateway_basename(gateway): matched = True if not matched: continue # Check names rule_name = rule.rule.name matched = self.is_matched(rule, rule_name, name, node) if not matched: rule_name = '/' + unique_name + '/' + rule.rule.name matched = self.is_matched(rule, rule_name, name, node) if not matched: rule_name = '/' + rule.rule.name matched = self.is_matched(rule, rule_name, name, node) if matched: matched_pull = copy.deepcopy(rule) matched_pull.gateway = gateway # just in case we used a regex or matched basename matched_pull.rule.name = name # just in case we used a regex matched_pull.rule.node = node # just in case we used a regex matched_pull_rules.append(matched_pull) return matched_pull_rules
def matches_remote_gateway_basename(self, gateway): ''' Use this when gateway can be a regular expression and we need to check it off against list_remote_gateway_names() ''' weak_matches = [] try: for remote_gateway in self.list_remote_gateway_names(): if re.match(gateway, rocon_utilities.gateway_basename(remote_gateway)): weak_matches.append(remote_gateway) except HubConnectionLostError: raise return weak_matches
def _generate_flips(self, connection_type, name, node, remote_gateways, unique_name): ''' Checks if a local rule (obtained from master.get_system_state) is a suitable association with any of the rules or patterns. This can return multiple matches, since the same local rule properties can be multiply flipped to different remote gateways. Used in the update() call above that is run in the watcher thread. Note, don't need to lock here as the update() function takes care of it. @param connection_type : rule type @type str : string constant from gateway_msgs.msg.Rule @param name : fully qualified topic, service or action name @type str @param node : ros node name (coming from master.get_system_state) @type str @param gateways : gateways that are available (registered on the hub) @type string @return all the flip rules that match this local rule @return list of RemoteRule objects updated with node names from self.watchlist ''' matched_flip_rules = [] for flip_rule in self.watchlist[connection_type]: # Check if the flip rule corresponds to an existing gateway matched_gateways = [] for gateway in remote_gateways: # check for regular expression or perfect match gateway_match_result = re.match(flip_rule.gateway, gateway) if gateway_match_result and gateway_match_result.group( ) == gateway: matched_gateways.append(gateway) elif flip_rule.gateway == rocon_utilities.gateway_basename( gateway): matched_gateways.append(gateway) if not matched_gateways: continue # Check names rule_name = flip_rule.rule.name matched = self.is_matched(flip_rule, rule_name, name, node) if not utils.is_all_pattern(flip_rule.rule.name): if not matched: rule_name = '/' + unique_name + '/' + flip_rule.rule.name matched = self.is_matched(flip_rule, rule_name, name, node) if not matched: rule_name = '/' + flip_rule.rule.name matched = self.is_matched(flip_rule, rule_name, name, node) if matched: for gateway in matched_gateways: matched_flip = copy.deepcopy(flip_rule) matched_flip.gateway = gateway # just in case we used a regex or matched basename matched_flip.rule.name = name # just in case we used a regex matched_flip.rule.node = node # just in case we used a regex matched_flip_rules.append(matched_flip) return matched_flip_rules
def test_basename(self): self.assertEquals("dude", rocon_utilities.gateway_basename('dude1285014a28c74162bf19952d1481197e'))