00001
00002
00003
00004
00005
00006
00007
00008
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
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()
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
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
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
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
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
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)
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]
00177 if hub.send_unflip_request(flip.gateway, rule):
00178
00179 hub.remove_flip_details(flip.gateway, flip.rule.name, flip.rule.type, flip.rule.node)
00180 break
00181
00182
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
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
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
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
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
00258
00259
00260
00261
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
00279 new_conns, lost_conns = self.public_interface.update(
00280 local_connection_index, self.master.generate_advertisement_connection_details)
00281
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
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
00331 for (registration, status) in registrations:
00332
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
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
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
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
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
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
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
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
00502
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
00510
00511 if not request.cancel:
00512 response = self._add_flip_rules(request.remotes)
00513 else:
00514 response = self._remove_flip_rules(request.remotes)
00515
00516
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:
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
00565
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
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:
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:
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:
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
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
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
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