def remote_gateway_info(self, gateway): ''' Return remote gateway information for the specified gateway string id. @param gateways : gateway id string to search for @type string @return remote gateway information @rtype gateway_msgs.RemotGateway or None ''' firewall = self._redis_server.get( hub_api.create_rocon_gateway_key(gateway, 'firewall')) ip = self._redis_server.get( hub_api.create_rocon_gateway_key(gateway, 'ip')) if firewall is None: return None # equivalent to saying no gateway of this id found else: remote_gateway = gateway_msgs.RemoteGateway() remote_gateway.name = gateway remote_gateway.ip = ip remote_gateway.firewall = True if int(firewall) else False remote_gateway.public_interface = [] encoded_advertisements = self._redis_server.smembers( hub_api.create_rocon_gateway_key(gateway, 'advertisements')) for encoded_advertisement in encoded_advertisements: advertisement = utils.deserialize_connection( encoded_advertisement) remote_gateway.public_interface.append(advertisement.rule) remote_gateway.flipped_interface = [] encoded_flips = self._redis_server.smembers( hub_api.create_rocon_gateway_key(gateway, 'flips')) for encoded_flip in encoded_flips: [target_gateway, name, connection_type, node] = utils.deserialize(encoded_flip) remote_rule = gateway_msgs.RemoteRule( target_gateway, gateway_msgs.Rule(connection_type, name, node)) remote_gateway.flipped_interface.append(remote_rule) remote_gateway.pulled_interface = [] encoded_pulls = self._redis_server.smembers( hub_api.create_rocon_gateway_key(gateway, 'pulls')) for encoded_pull in encoded_pulls: [target_gateway, name, connection_type, node] = utils.deserialize(encoded_pull) remote_rule = gateway_msgs.RemoteRule( target_gateway, gateway_msgs.Rule(connection_type, name, node)) remote_gateway.pulled_interface.append(remote_rule) return remote_gateway
def flip(gate, remote, local, topic, node): rospy.init_node('generic_flip_subscriber') remote_gateway = gate[0] flip_service = rospy.ServiceProxy('/gateway/flip', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] rule = gateway_msgs.Rule() if topic[0][0] == '/': rule.name = topic[0] elif not remote: rule.name = '/' + topic[0] else: rule.name = '/' + remote[0] + '/' + topic[0] rule.type = gateway_msgs.ConnectionType.SUBSCRIBER if not node: rule.node = '' elif node[0][0] == '/': rule.node = node[0] elif not local: rule.node = '/' + node[0] else: rule.node = '/' + local[0] + '/' + node[0] req.remotes.append(gateway_msgs.RemoteRule(remote_gateway, rule)) rospy.loginfo("Flip : [%s,%s,%s,%s]." % (remote_gateway, rule.name, rule.type, rule.node)) resp = flip_service(req) if resp.result != 0: rospy.logerr("Flip : %s" % resp.error_message) rospy.loginfo("Finished flipping connection.")
def pull(gate, remote, local, topic, node): rospy.init_node('generic_puller') rospy.wait_for_service('/gateway/pull') remote_gateway = gate[0] pull_service = rospy.ServiceProxy('/gateway/pull', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] rule = gateway_msgs.Rule() if topic[0][0] == '/': rule.name = topic[0] elif not remote: rule.name = '/' + topic[0] ; else: rule.name = '/' + remote[0] + '/' + topic[0] rule.type = gateway_msgs.ConnectionType.PUBLISHER if not node: rule.node = '' elif node[0][0] == '/': rule.node = node[0] elif not local: rule.node = '/' + node[0] else: rule.node = '/' + local[0] + '/' + node[0] req.remotes.append(gateway_msgs.RemoteRule(remote_gateway, rule)) rospy.loginfo("Pull : [%s,%s,%s,%s]."%(remote_gateway, rule.name, rule.type, rule.node)) resp = pull_service(req) if resp.result != 0: rospy.logerr("Pull : %s"%resp.error_message) rospy.loginfo("Finished pulling connection.")
def _pull_concert_client(self): ''' Pulls platform information from the advertised platform_info on another ros system. It is just a one-shot only used at construction time. It pulls platform_info and list_apps information. It also pulls the required handles in for further manipulation ('status' and 'invite') ''' rospy.loginfo("Conductor: retrieving client information [%s]" % self.name) pull_service = rospy.ServiceProxy('~pull', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] for service_name in ['platform_info', 'list_apps', 'status', 'invite']: rule = gateway_msgs.Rule() rule.name = str('/' + self.gateway_name + '/' + service_name) rule.node = '' rule.type = gateway_msgs.ConnectionType.SERVICE req.remotes.append( gateway_msgs.RemoteRule(self.gateway_name.lstrip('/'), rule)) resp = pull_service(req) if resp.result != 0: rospy.logwarn( "Conductor: failed to pull the platform info service from the client." ) return None
def flip_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace): rospy.wait_for_service(ns + '/flip') flip = rospy.ServiceProxy(ns + '/flip', gateway_srvs.Remote) if not remote_gateway_name: remote_gateway_name = find_first_remote_gateway() req = gateway_srvs.RemoteRequest() req.cancel = cancel if regex_patterns: names, nodes = create_tutorial_dictionaries(use_regex_patterns=True) else: names, nodes = create_tutorial_dictionaries(use_regex_patterns=False) req.remotes = [] for connection_type in connection_types: rule = gateway_msgs.Rule() rule.name = names[connection_type] rule.type = connection_type rule.node = nodes[connection_type] rospy.loginfo( "Flip : %s [%s,%s,%s][%s]." % (_action_text(cancel, 'requesting flip to gateway'), rule.type, rule.name, rule.node or 'None', remote_gateway_name)) req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule)) resp = flip(req) if resp.result == gateway_msgs.ErrorCodes.FLIP_RULE_ALREADY_EXISTS: # already flipped all error is ignored. this call only has no effect. pass elif resp.result != 0: raise GatewaySampleRuntimeError("failed to flip %s [%s]" % (rule.name, resp.error_message))
def _send_flip_rules(self, robots, cancel): """ Flip rules from Gazebo to the robot's concert client. :param robot_names str[]: Names of robots to whom information needs to be flipped. :param cancel bool: Cancel existing flips. Used during shutdown. """ for robot in robots: rules = self._robot_managers[robot['type']].get_flip_rule_list( robot['name']) # send the request request = gateway_srvs.RemoteRequest() request.cancel = cancel remote_rule = gateway_msgs.RemoteRule() remote_rule.gateway = robot['name'] for rule in rules: remote_rule.rule = rule request.remotes.append(copy.deepcopy(remote_rule)) try: self._gateway_flip_service(request) except rospy.ServiceException: # communication failed self.logerr('failed to send flip rules') return except rospy.ROSInterruptException: self.loginfo( 'shutdown while contacting the gateway flip service') return
def pull_tutorials(remote_gateway_name=None, cancel=False, regex_patterns=False, ns=_gateway_namespace): rospy.wait_for_service(ns + '/pull') pull = rospy.ServiceProxy(ns + '/pull', gateway_srvs.Remote) if not remote_gateway_name: remote_gateway_name = find_first_remote_gateway() req = gateway_srvs.RemoteRequest() req.cancel = cancel if regex_patterns: names, nodes = create_tutorial_dictionaries(use_regex_patterns=True) else: names, nodes = create_tutorial_dictionaries(use_regex_patterns=False) req.remotes = [] for connection_type in connection_types: rule = gateway_msgs.Rule() rule.name = names[connection_type] rule.type = connection_type rule.node = nodes[connection_type] rospy.loginfo( "Pull : %s [%s,%s,%s][%s]." % (_action_text(cancel, 'sending pull rule to the gateway'), rule.type, rule.name, rule.node or 'None', remote_gateway_name)) req.remotes.append(gateway_msgs.RemoteRule(remote_gateway_name, rule)) resp = pull(req) if resp.result != 0: raise GatewaySampleRuntimeError("failed to pull %s [%s]" % (rule.name, resp.error_message))
def _send_flip_rules_request(self, name, cancel): rules = [] rule = gateway_msgs.Rule() rule.node = '' rule.type = gateway_msgs.ConnectionType.SUBSCRIBER # could resolve this better by looking up the service info rule.name = "/services/turtlesim/%s/cmd_vel" % name rules.append(copy.deepcopy(rule)) rule.type = gateway_msgs.ConnectionType.PUBLISHER rule.name = "/services/turtlesim/%s/pose" % name rules.append(copy.deepcopy(rule)) # send the request request = gateway_srvs.RemoteRequest() request.cancel = cancel remote_rule = gateway_msgs.RemoteRule() remote_rule.gateway = name for rule in rules: remote_rule.rule = rule request.remotes.append(copy.deepcopy(remote_rule)) try: self._gateway_flip_service(request) except rospy.ServiceException: # communication failed rospy.logerr("TurtleHerder : failed to send flip rules") return except rospy.ROSInterruptException: rospy.loginfo("TurtleHerder : shutdown while contacting the gateway flip service") return
def request_pulls(self, remote_gateway_name, cancel=False, service_names=['platform_info', 'list_rapps', 'invite'], topic_names=['status']): """ Handles pull requests and cancels from request gateways for the conductor. Note this only applies to topics/services relevant for interacting with concert clients. :param str remote_gateway_name: name of a remote gateway to apply to all rules :param bool cancel: to register or unregister the pull requests """ req = gateway_srvs.RemoteRequest() req.cancel = cancel req.remotes = [] for service_name in service_names: rule = gateway_msgs.Rule() rule.name = str('/' + remote_gateway_name.lower().replace(' ', '_') + '/' + service_name) rule.node = '' rule.type = gateway_msgs.ConnectionType.SERVICE req.remotes.append( gateway_msgs.RemoteRule(remote_gateway_name.lstrip('/'), rule)) for publisher_name in topic_names: rule = gateway_msgs.Rule() rule.name = str('/' + remote_gateway_name.lower().replace(' ', '_') + '/' + publisher_name) rule.node = '' rule.type = gateway_msgs.ConnectionType.PUBLISHER req.remotes.append( gateway_msgs.RemoteRule(remote_gateway_name.lstrip('/'), rule)) # TODO : exception handling for this call response = self._services['pull'](req) if response.result != gateway_msgs.ErrorCodes.SUCCESS and not cancel: # don't worry about errors on cleanup rospy.logwarn( "Conductor: failed to register pull requests from the concert client [%s]%s" % (remote_gateway_name, service_names) ) # TODO : exceptions, but what kind of failures?
def pull(self, type): rule = gateway_msgs.Rule() rule.name = self.names[type] rule.type = type rule.node = self.nodes[type] self.req.remotes.append(gateway_msgs.RemoteRule(self.gateway, rule)) resp = self.pull_service(self.req) if resp.result == gateway_msgs.ErrorCodes.SUCCESS: rospy.loginfo("Pull : %s [%s,%s,%s,%s]." % (self.action_text, self.gateway, rule.type, rule.name, rule.node or 'None')) else: rospy.logerr("Pull : pull failed [%s]" % str(resp.error_message)) self.req.remotes = []
def cancel_pulls(self): ''' Cancel any pulls for a client which failed to construct (e.g. in the init() when the service calls fail. ''' pull_service = rospy.ServiceProxy('~pull', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = True req.remotes = [] for service_name in ['platform_info', 'list_apps', 'status', 'invite']: rule = gateway_msgs.Rule() rule.name = str('/' + self.gateway_name + '/' + service_name) rule.node = '' rule.type = gateway_msgs.ConnectionType.SERVICE req.remotes.append( gateway_msgs.RemoteRule(self.gateway_name.lstrip('/'), rule)) unused_resp = pull_service(req)
def flip(namespace, topic, node): rospy.init_node('flip_remote_publisher') remote_gateway = "Hub_Gateway" flip_service = rospy.ServiceProxy('/gateway/flip', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] rule = gateway_msgs.Rule() rule.name = '/' + namespace + '/' + topic rule.type = gateway_msgs.ConnectionType.PUBLISHER rule.node = '/' + namespace + '/' + node req.remotes.append(gateway_msgs.RemoteRule(remote_gateway, rule)) rospy.loginfo("Flip : [%s,%s,%s,%s]." % (remote_gateway, rule.name, rule.type, rule.node)) resp = flip_service(req) if resp.result != 0: rospy.logerr("Flip : %s" % resp.error_message) rospy.loginfo("Finished flipping connection.")
def _send_flip_rules(self, remote_controller): request = gateway_srvs.RemoteRequest() request.cancel = True if remote_controller == rocon_app_manager_msgs.Constants.NO_REMOTE_CONTROLLER else False remote_rule = gateway_msgs.RemoteRule() if request.cancel: remote_rule.gateway = self.remote_controller else: remote_rule.gateway = remote_controller for rule in self._flip_rules(): remote_rule.rule = rule request.remotes.append(copy.deepcopy(remote_rule)) try: unused_response = self.gateway_flip_service(request) except rospy.ServiceException: # communication failed rospy.logerr("Hatchling : failed to send flip rules") return except rospy.ROSInterruptException: rospy.loginfo( "Hatchling : shutdown while contacting the gateway flip service" ) return
def pull(topic): rospy.init_node('pull_hub_publisher') rospy.wait_for_service('/gateway/pull') remote_gateway = "Hub_Gateway" pull_service = rospy.ServiceProxy('/gateway/pull', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] rule = gateway_msgs.Rule() rule.name = '/' + topic rule.type = gateway_msgs.ConnectionType.PUBLISHER rule.node = "" req.remotes.append(gateway_msgs.RemoteRule(remote_gateway, rule)) rospy.loginfo("Pull : [%s,%s,%s,%s]." % (remote_gateway, rule.name, rule.type, rule.node)) resp = pull_service(req) if resp.result != 0: rospy.logerr("Pull : %s" % resp.error_message) rospy.loginfo("Finished pulling connection.")
def flip(namespace, topic, node, gate): rospy.init_node('flip_hub_subscriber') remote_gateway = gate + '_gateway' flip_service = rospy.ServiceProxy('/gateway/flip', gateway_srvs.Remote) req = gateway_srvs.RemoteRequest() req.cancel = False req.remotes = [] rule = gateway_msgs.Rule() if topic[0] == '/': rule.name = topic else: rule.name = '/' + namespace + '/' + topic rule.type = gateway_msgs.ConnectionType.SUBSCRIBER rule.node = '/' + node req.remotes.append(gateway_msgs.RemoteRule(remote_gateway, rule)) rospy.loginfo("Flip : [%s,%s,%s,%s]." % (remote_gateway, rule.name, rule.type, rule.node)) resp = flip_service(req) if resp.result != 0: rospy.logerr("Flip : %s" % resp.error_message) rospy.loginfo("Finished flipping connection.")
def remote_gateway_info(self, gateway): ''' Return remote gateway information for the specified gateway string id. @param gateways : gateway id string to search for @type string @return remote gateway information @rtype gateway_msgs.RemotGateway or None ''' firewall = self._redis_server.get(hub_api.create_rocon_gateway_key(gateway, 'firewall')) if firewall is None: return None # equivalent to saying no gateway of this id found ip = self._redis_server.get(hub_api.create_rocon_gateway_key(gateway, 'ip')) if ip is None: return None # hub information not available/correct remote_gateway = gateway_msgs.RemoteGateway() remote_gateway.name = gateway remote_gateway.ip = ip remote_gateway.firewall = True if int(firewall) else False remote_gateway.public_interface = [] encoded_advertisements = self._redis_server.smembers( hub_api.create_rocon_gateway_key(gateway, 'advertisements')) for encoded_advertisement in encoded_advertisements: advertisement = utils.deserialize_connection(encoded_advertisement) remote_gateway.public_interface.append(advertisement.rule) remote_gateway.flipped_interface = [] encoded_flips = self._redis_server.smembers(hub_api.create_rocon_gateway_key(gateway, 'flips')) for encoded_flip in encoded_flips: [target_gateway, name, connection_type, node] = utils.deserialize(encoded_flip) remote_rule = gateway_msgs.RemoteRule(target_gateway, gateway_msgs.Rule(connection_type, name, node)) remote_gateway.flipped_interface.append(remote_rule) remote_gateway.pulled_interface = [] encoded_pulls = self._redis_server.smembers(hub_api.create_rocon_gateway_key(gateway, 'pulls')) for encoded_pull in encoded_pulls: [target_gateway, name, connection_type, node] = utils.deserialize(encoded_pull) remote_rule = gateway_msgs.RemoteRule(target_gateway, gateway_msgs.Rule(connection_type, name, node)) remote_gateway.pulled_interface.append(remote_rule) # Gateway health/network connection statistics indicators gateway_available_key = hub_api.create_rocon_gateway_key(gateway, 'available') remote_gateway.conn_stats.gateway_available = \ self._parse_redis_bool(self._redis_server.get(gateway_available_key)) time_since_last_seen_key = hub_api.create_rocon_gateway_key(gateway, 'time_since_last_seen') remote_gateway.conn_stats.time_since_last_seen = \ self._parse_redis_int(self._redis_server.get(time_since_last_seen_key)) ping_latency_min_key = hub_api.create_rocon_gateway_key(gateway, 'latency:min') remote_gateway.conn_stats.ping_latency_min = \ self._parse_redis_float(self._redis_server.get(ping_latency_min_key)) ping_latency_max_key = hub_api.create_rocon_gateway_key(gateway, 'latency:max') remote_gateway.conn_stats.ping_latency_max = \ self._parse_redis_float(self._redis_server.get(ping_latency_max_key)) ping_latency_avg_key = hub_api.create_rocon_gateway_key(gateway, 'latency:avg') remote_gateway.conn_stats.ping_latency_avg = \ self._parse_redis_float(self._redis_server.get(ping_latency_avg_key)) ping_latency_mdev_key = hub_api.create_rocon_gateway_key(gateway, 'latency:mdev') remote_gateway.conn_stats.ping_latency_mdev = \ self._parse_redis_float(self._redis_server.get(ping_latency_mdev_key)) # Gateway network connection indicators network_info_available_key = hub_api.create_rocon_gateway_key(gateway, 'network:info_available') remote_gateway.conn_stats.network_info_available = \ self._parse_redis_bool(self._redis_server.get(network_info_available_key)) if not remote_gateway.conn_stats.network_info_available: return remote_gateway network_type_key = hub_api.create_rocon_gateway_key(gateway, 'network:type') remote_gateway.conn_stats.network_type = \ self._parse_redis_int(self._redis_server.get(network_type_key)) if remote_gateway.conn_stats.network_type == gateway_msgs.RemoteGateway.WIRED: return remote_gateway wireless_bitrate_key = hub_api.create_rocon_gateway_key(gateway, 'wireless:bitrate') remote_gateway.conn_stats.wireless_bitrate = \ self._parse_redis_float(self._redis_server.get(wireless_bitrate_key)) wireless_link_quality_key = hub_api.create_rocon_gateway_key(gateway, 'wireless:quality') remote_gateway.conn_stats.wireless_link_quality = \ self._parse_redis_int(self._redis_server.get(wireless_link_quality_key)) wireless_signal_level_key = hub_api.create_rocon_gateway_key(gateway, 'wireless:signal_level') remote_gateway.conn_stats.wireless_signal_level = \ self._parse_redis_float(self._redis_server.get(wireless_signal_level_key)) wireless_noise_level_key = hub_api.create_rocon_gateway_key(gateway, 'wireless:noise_level') remote_gateway.conn_stats.wireless_noise_level = \ self._parse_redis_float(self._redis_server.get(wireless_noise_level_key)) return remote_gateway
def create_gateway_remote_rule(gateway, rule): r = gateway_msgs.RemoteRule() r.gateway = gateway r.rule = rule return r