def test_wildcards(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Wildcards" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) rocon_uri_string = 'rocon:/turtlebot2/dude/hydro/precise#rocon_apps/chirp' hardware_platform_uri = rocon_uri_string.replace('turtlebot2', '*') print(console.cyan + " - %s" % hardware_platform_uri + console.reset) rocon_uri_object = rocon_uri.parse(hardware_platform_uri) assert rocon_uri_object.hardware_platform.string == '*' operating_systems_uri = rocon_uri_string.replace('precise', '*') print(console.cyan + " - %s" % operating_systems_uri + console.reset) rocon_uri_object = rocon_uri.parse(operating_systems_uri) assert rocon_uri_object.operating_system.string == '*' application_framework_uri = rocon_uri_string.replace('hydro', '*') print(console.cyan + " - %s" % application_framework_uri + console.reset) rocon_uri_object = rocon_uri.parse(application_framework_uri) assert rocon_uri_object.application_framework.string == '*' name_uri = rocon_uri_string.replace('dude', '*') print(console.cyan + " - %s" % name_uri + console.reset) rocon_uri_object = rocon_uri.parse(name_uri) assert rocon_uri_object.name.string == '*'
def test_missing_fields(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Missing Fields" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) rocon_uri_string = 'rocon:/turtlebot2/dude/hydro/precise' no_operating_system = 'rocon:/turtlebot2/dude/hydro' rocon_uri_object = rocon_uri.parse(no_operating_system) print(console.cyan + " - %s -> %s" % (no_operating_system, rocon_uri_object) + console.reset) assert (rocon_uri_object.operating_system.list[0] == '*') no_application_framework = 'rocon:/turtlebot2/dude' rocon_uri_object = rocon_uri.parse(no_application_framework) print(console.cyan + " - %s -> %s" % (no_application_framework, rocon_uri_object) + console.reset) assert (rocon_uri_object.application_framework.list[0] == '*') no_name = 'rocon:/turtlebot2' rocon_uri_object = rocon_uri.parse(no_name) print(console.cyan + " - %s -> %s" % (no_name, rocon_uri_object) + console.reset) assert (rocon_uri_object.name.list[0] == '*')
def _flag_resource_trackers(self, resources, tracking, allocated, high_priority_flag=False): ''' Update the flags in the resource trackers for each resource. This is used to follow whether a particular resource is getting tracked, or is already allocated so that we can determine if new requests should be made and at what priority. ''' for resource in resources: # do a quick check to make sure individual resources haven't been previously allocated, and then lost # WARNING : this unallocated check doesn't actually work - the requester isn't sending us back this info yet. if rocon_uri.parse( resource.uri ).name.string == concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE: tracking = False allocated = False resource_tracker = self._find_resource_tracker( unique_id.toHexString(resource.id)) if resource_tracker is None: pass # should raise an exception else: resource_tracker.tracking = tracking resource_tracker.allocated = allocated resource_tracker.high_priority_flag = high_priority_flag
def __init__(self, stop_app_postexec_fn): ''' @param stop_app_postexec_fn : callback to fire when a listener detects an app getting stopped. @type method with no args ''' self._stop_app_postexec_fn = stop_app_postexec_fn self.app_list = {} self.rocon_master_info = {} self.is_connect = False self.is_app_running = False self.key = uuid.uuid4() self.app_pid = 0 # this might be naive and only work well on ubuntu... os_codename = OsDetect().get_codename() # this would be great as a configurable parameter name = "rqt_remocon_" + self.key.hex self.rocon_uri = rocon_uri.parse( "rocon:/pc/" + name + "/" + rocon_std_msgs.Strings.URI_WILDCARD + "/" + os_codename ) # be also great to have a configurable icon...with a default self.platform_info = rocon_std_msgs.PlatformInfo(version=rocon_std_msgs.Strings.ROCON_VERSION, uri=str(self.rocon_uri), icon=rocon_std_msgs.Icon() ) print("[remocon_info] info component initialised")
def requester_feedback(self, request_set): ''' Keep an eye on our pending requests and see if they get allocated here. Once they do, kick them out of the pending requests list so _ros_capture_teleop_callback can process and reply to the interaction. @param request_set : the modified requests @type dic { uuid.UUID : scheduler_msgs.ResourceRequest } ''' cancelled_requests = 0 for request_id, request in request_set.requests.iteritems(): if request_id in self.pending_requests: if request.msg.status == scheduler_msgs.Request.GRANTED: self.pending_requests.remove(request_id) self.allocated_requests.append(request_id) elif request.msg.status == scheduler_msgs.Request.GRANTED: completely_unallocated = True for resource in request.msg.resources: if rocon_uri.parse(resource.uri).name.string != concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE: completely_unallocated = False if completely_unallocated: cancelled_requests += 1 request.cancel() # Need to add logic here for when the request gets released for unused_i in range(0, cancelled_requests): self.request_turtle()
def requester_feedback(self, request_set): ''' Keep an eye on our pending requests and see if they get allocated here. Once they do, kick them out of the pending requests list so _ros_capture_teleop_callback can process and reply to the interaction. @param request_set : the modified requests @type dic { uuid.UUID : scheduler_msgs.ResourceRequest } ''' cancelled_requests = 0 for request_id, request in request_set.requests.iteritems(): if request_id in self.pending_requests: if request.msg.status == scheduler_msgs.Request.GRANTED: self.pending_requests.remove(request_id) self.allocated_requests.append(request_id) elif request.msg.status == scheduler_msgs.Request.GRANTED: completely_unallocated = True for resource in request.msg.resources: if rocon_uri.parse(resource.uri).name.string != concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE: completely_unallocated = False if completely_unallocated: cancelled_requests += 1 request.cancel() # Need to add logic here for when the request gets released for unused_i in range(0, cancelled_requests): self.request_turtle()
def _ros_subscriber_concert_client_changes(self, msg): """ Receives a list of all concert clients every time the state of the list changes (i.e. not periodically) @param msg : concert_msgs.ConcertClients """ self._lock.acquire() # new_clients: concert_msgs.ConcertClient[] new_clients = [ client for client in msg.clients if client.gateway_name not in self._clients.keys() ] # lost_clients: common.ConcertClient[] lost_clients = [ client for client in self._clients.values() if client.gateway_name not in [c.gateway_name for c in msg.clients] ] # work over the client list for client in new_clients: rospy.loginfo("Scheduler : new concert client [%s]" % client.name) self._clients[client.gateway_name] = common.ConcertClient( client) # default setting is unallocated for client in lost_clients: if client.allocated: rospy.logwarn( "Scheduler : lost allocated concert client [%s]" % client.name) # WARNINGg, this code actually doesn't work - it doesn't force the scheduler to send the updated information # back to the requester. Instead, the requester sends its heartbeat updates, which end up overwriting these # changes in _requester_update. client_platform_uri = rocon_uri.parse( client.msg.platform_info.uri) client_platform_uri.name = client.msg.gateway_name for request_set in self._request_sets.values(): found = False for request in request_set.values(): for resource in request.msg.resources: # could use a better way to check for equality than this if resource.uri == str(client_platform_uri): client_platform_uri.name = concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE resource.uri = str(client_platform_uri) found = True break if found: self._scheduler.notify(request_set.requester_id) # @todo might want to consider changing the request status if all resources have been unallocated break if found: break # @todo might want to validate that we unallocated since we did detect an allocated flag del self._clients[client.gateway_name] if new_clients or lost_clients: self._publish_resource_pool() # should check for some things here, e.g. can we verify a client is allocated or not? self._update() self._lock.release()
def _create_resource(self, uri): # Create a resource to request resource = scheduler_msgs.Resource() resource.id = unique_id.toMsg(unique_id.fromRandom()) resource.rapp = self.resource_type resource.uri = uri compressed_image_topic_remapped = self._get_remapped_topic(rocon_uri.parse(resource.uri).name.string) resource.remappings = [rocon_std_msgs.Remapping(self._default_compressed_image_topic, compressed_image_topic_remapped)] return resource
def _uninit_teleop(self, captured_teleop_rocon_uri): """ Shutdown the teleop interface for the captured robot. :param str captured_teleop_rocon_uri: captured rocon uri """ unused_uri = rocon_uri.parse(captured_teleop_rocon_uri) with self._lock: if self.teleop_interface: self.teleop_interface.shutdown() self.teleop_interface = None
def _init_teleop(self, captured_teleop_rocon_uri): """ After capturing teleop, intialization with teleop information. @param teleop_name : captured teleop name @type string """ uri = rocon_uri.parse(captured_teleop_rocon_uri) captured_name = uri.name.string self.captured_teleop_cmd_vel_pub = rospy.Publisher(captured_name + "/cmd_vel", Twist, latch=True) self.captured_teleop_compressed_image_sub = rospy.Subscriber(captured_name + "/compressed_image", CompressedImage, self._update_teleop_image)
def _refresh_resource_list(self, resource_list): self._lock.acquire() self.resource_list_tree_widget.clear() self.resource_item_list = {} for r in resource_list: uri = rocon_uri.parse(r) resource_item = QTreeWidgetItem(self.resource_list_tree_widget) resource_item.setText(0, uri.name.string) self.resource_item_list[resource_item] = r self._lock.release()
def test_multiple_elements(): ''' Test the OR functionality. ''' print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* Parsing multiple elements" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) rocon_uri_string = 'rocon:/turtlebot2/dude/hydro/precise#rocon_apps/chirp' multiple_hardware_platforms = rocon_uri_string.replace( 'turtlebot2', 'turtlebot2|pr2|waiterbot') print(console.cyan + " - %s" % multiple_hardware_platforms + console.reset) rocon_uri_object = rocon_uri.parse(multiple_hardware_platforms) print(console.yellow + " - %s" % rocon_uri_object.hardware_platform.list + console.reset) assert len(rocon_uri_object.hardware_platform.list) == 3 multiple_operating_systems = rocon_uri_string.replace( 'precise', 'quantal|precise') print(console.cyan + " - %s" % multiple_operating_systems + console.reset) rocon_uri_object = rocon_uri.parse(multiple_operating_systems) print(console.yellow + " - %s" % rocon_uri_object.operating_system.list + console.reset) assert len(rocon_uri_object.operating_system.list) == 2 multiple_application_frameworks = rocon_uri_string.replace( 'hydro', 'hydro|application_framework_other') print(console.cyan + " - %s" % multiple_application_frameworks + console.reset) rocon_uri_object = rocon_uri.parse(multiple_application_frameworks) print(console.yellow + " - %s" % rocon_uri_object.application_framework.list + console.reset) assert len(rocon_uri_object.application_framework.list) == 2
def _uninit_teleop(self, captured_teleop_rocon_uri): """ After release teleop, unintialization with teleop information. @param teleop_name : captured teleop name @type string """ uri = rocon_uri.parse(captured_teleop_rocon_uri) self.image_data = None self.captured_teleop_cmd_vel_pub.unregister() self.captured_teleop_compressed_image_sub.unregister() self.captured_teleop_cmd_vel_pub = None self.captured_teleop_compressed_image_sub = None
def _init_teleop(self, captured_teleop_rocon_uri): """ Initialise the teleop interface for a newly captured robot. :param str captured_teleop_rocon_uri: captured rocon uri """ captured_name = rocon_uri.parse(captured_teleop_rocon_uri).name.string with self._lock: self.teleop_interface = TeleopInterface( image_received_slot=self._image_received_slot, cmd_vel_topic_name=captured_name + "/cmd_vel", compressed_image_topic_name=captured_name + "/compressed_image" )
def _gen_resource(self, node, edges): """ To convert linkgraph information for a particular node to a scheduler_msgs.Resource type :param node: :param edges: :return: """ resource = scheduler_msgs.Resource() resource.rapp = rocon_uri.parse(node.resource).rapp resource.uri = node.resource resource.remappings = [rocon_std_msgs.Remapping(e.remap_from, e.remap_to) for e in edges if e.start == node.id or e.finish == node.id] return resource
def request_completely_unallocated(request): ''' Quick check to see if a request's resources have been entirely lost (i.e. unallocated). This will trigger the responder to cleanup (i.e. release the request). @param resources @type scheduler_msgs.Request @return true or false if entirely unallocated or not. ''' for resource in request.msg.resources: if rocon_uri.parse(resource.uri).name.string != concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE: return False return True
def test_stringify(): print( console.bold + "\n****************************************************************************************" + console.reset) print(console.bold + "* String representation" + console.reset) print( console.bold + "****************************************************************************************" + console.reset) rocon_uri_string = 'rocon:/turtlebot2/dude/hydro/precise#rocon_apps/chirp' print(console.cyan + " - %s" % rocon_uri_string + console.reset) rocon_uri_object = rocon_uri.parse(rocon_uri_string) assert str(rocon_uri_object) == rocon_uri_string
def _node_to_resource(self, node, linkgraph): ''' Convert linkgraph information for a particular node to a scheduler_msgs.Resource type. @param node : a node from the linkgraph @type concert_msgs.LinkNode @param linkgraph : the entire linkgraph (used to lookup the node's edges) @type concert_msgs.LinkGraph @return resource @rtype scheduler_msgs.Resource ''' resource = scheduler_msgs.Resource() resource.rapp = rocon_uri.parse(node.resource).rapp resource.uri = node.resource resource.remappings = [rocon_std_msgs.Remapping(e.remap_from, e.remap_to) for e in linkgraph.edges if e.start == node.id or e.finish == node.id] return resource
def _node_to_resource(node, linkgraph): ''' Convert linkgraph information for a particular node to a scheduler_msgs.Resource type. @param node : a node from the linkgraph @type concert_msgs.LinkNode @param linkgraph : the entire linkgraph (used to lookup the node's edges) @type concert_msgs.LinkGraph @return resource @rtype scheduler_msgs.Resource ''' resource = scheduler_msgs.Resource() resource.rapp = rocon_uri.parse(node.resource).rapp resource.uri = node.resource resource.remappings = [rocon_std_msgs.Remapping(e.remap_from, e.remap_to) for e in linkgraph.edges if e.start == node.id or e.finish == node.id] resource.parameters = [rocon_std_msgs.KeyValue(key,str(val)) for key, val in node.parameters.items()] return resource
def interactions_found(self): # self.namespace_scanner.busy_dialog.cancel() self.namespaces = self.namespace_scanner.namespaces print("Namespaces %s" % self.namespaces) self.active_namespace = None if not self.namespaces else self.namespaces[ 0] self.rocon_uri = rocon_uri.parse( rocon_uri.generate_platform_rocon_uri('pc', self.name) + "|" + utils.get_web_browser_codename()) self.active_pairing = None self.active_paired_interaction_hashes = [] # be also great to have a configurable icon...with a default self.platform_info = rocon_std_msgs.MasterInfo( version=rocon_std_msgs.Strings.ROCON_VERSION, rocon_uri=str(self.rocon_uri), icon=rocon_std_msgs.Icon(), description="") # Load Data self.pairings_table = get_pairings(self.active_namespace) self.interactions_table = get_interactions(self.active_namespace, "rocon:/") self.service_proxies = rocon_python_comms.utils.ServiceProxies([ (self.active_namespace + "request_interaction", interaction_srvs.RequestInteraction), (self.active_namespace + "start_pairing", interaction_srvs.StartPairing), (self.active_namespace + "stop_pairing", interaction_srvs.StopPairing) ]) self.subscribers = rocon_python_comms.utils.Subscribers([ (self.active_namespace + "pairing_status", interaction_msgs.PairingStatus, self._subscribe_pairing_status_callback) ]) console.logdebug( "Remocon : interactions namespace found -> signalling the ui.") self.signal_updated.emit() self._publish_remocon_status()
def update(self, remote_gateway_info): """ Common updates for clients that have been observed on the gateway network. This handles updates from both the incoming gateway information (remote_gateway_info) as well as the cached rapp manager information (self._cached_status_msg). Updates primarily go into the concert client message data (self.msg). :param gateway_msgs.RemoteGateway remote_gateway_info: latest message providing up to date connectivity information about this concert client. :returns: success or failure of the update :rtype: bool """ is_changed = False # something changed self.touch() # remember, don't flag connection stats as worthy of a change. self.msg.conn_stats = remote_gateway_info.conn_stats #self.msg.last_connection_timestamp = rospy.Time.now() # do we really need this? # don't update every client, just the ones that we need information from important_state = (self.state == ConcertClient.State.AVAILABLE) or ( self.state == ConcertClient.State.UNINVITED) or ( self.state == ConcertClient.State.MISSING) if self._cached_status_msg is not None: with self._lock: status_msg = copy.deepcopy(self._cached_status_msg) self._cached_status_msg = None if important_state: # uri update uri = rocon_uri.parse(self.msg.platform_info.uri) uri.rapp = status_msg.rapp.name if status_msg.rapp_status == rapp_manager_msgs.Status.RAPP_RUNNING else '' self.msg.platform_info.uri = str(uri) is_changed = True if self._remote_controller != status_msg.remote_controller: self._remote_controller = status_msg.remote_controller is_changed = True return is_changed
def __init__(self, stop_interaction_postexec_fn): ''' @param stop_app_postexec_fn : callback to fire when a listener detects an app getting stopped. @type method with no args ''' self._interactions_table = InteractionsTable() self._stop_interaction_postexec_fn = stop_interaction_postexec_fn self.is_connect = False self.key = uuid.uuid4() self._ros_master_port = None try: self._roslaunch_terminal = rocon_launch.create_terminal() except (rocon_launch.UnsupportedTerminal, rocon_python_comms.NotFoundException) as e: console.warning( "Cannot find a suitable terminal, falling back to the current terminal [%s]" % str(e)) self._roslaunch_terminal = rocon_launch.create_terminal( rocon_launch.terminals.active) # this might be naive and only work well on ubuntu... os_codename = OsDetect().get_codename() webbrowser_codename = utils.get_web_browser_codename() # this would be good as a persistant variable so the user can set something like 'Bob' self.name = "rqt_remocon_" + self.key.hex self.rocon_uri = rocon_uri.parse("rocon:/pc/" + self.name + "/" + rocon_std_msgs.Strings.URI_WILDCARD + "/" + os_codename + "|" + webbrowser_codename) # be also great to have a configurable icon...with a default self.platform_info = rocon_std_msgs.PlatformInfo( version=rocon_std_msgs.Strings.ROCON_VERSION, uri=str(self.rocon_uri), icon=rocon_std_msgs.Icon()) console.logdebug("Interactive Client : initialised") self.pairing = None # if an interaction is currently pairing, this will store its hash # expose underlying functionality higher up self.interactions = self._interactions_table.generate_role_view """Get a dictionary of interactions belonging to the specified role."""
def __init__(self, stop_interaction_postexec_fn): ''' @param stop_app_postexec_fn : callback to fire when a listener detects an app getting stopped. @type method with no args ''' self._interactions_table = InteractionsTable() self._stop_interaction_postexec_fn = stop_interaction_postexec_fn self.is_connect = False self.key = uuid.uuid4() self._ros_master_port = None try: self._roslaunch_terminal = rocon_launch.create_terminal() except (rocon_launch.UnsupportedTerminal, rocon_python_comms.NotFoundException) as e: console.warning( "Cannot find a suitable terminal, falling back to the current terminal [%s]" % str(e)) self._roslaunch_terminal = rocon_launch.create_terminal( rocon_launch.terminals.active) # this would be good as a persistant variable so the user can set something like 'Bob' self.name = "rqt_remocon_" + self.key.hex self.rocon_uri = rocon_uri.parse( rocon_uri.generate_platform_rocon_uri('pc', self.name) + "|" + utils.get_web_browser_codename()) # be also great to have a configurable icon...with a default self.platform_info = rocon_std_msgs.MasterInfo( version=rocon_std_msgs.Strings.ROCON_VERSION, rocon_uri=str(self.rocon_uri), icon=rocon_std_msgs.Icon(), description="") self.currently_pairing_interaction_hashes = [] self.active_one_sided_interaction = None # expose underlying functionality higher up self.interactions = self._interactions_table.generate_role_view """Get a dictionary of interactions belonging to the specified role."""
def _update_robot_list(self, data): """ Update the available teleop robot list @param data: information of robot list @type """ for k in data.strings: uri = rocon_uri.parse(k) self.robot_list[k] = {} self.robot_list[k]["rocon_uri"] = k self.robot_list[k]["name"] = uri.name self.robot_list[k]["hardware_platform"] = uri.hardware_platform self.robot_list[k]["application_framework"] = uri.application_framework self.robot_list[k]["operating_system"] = uri.operating_system if len(self.pre_robot_list): if not self._compare_list(): self._event_callback() self.pre_robot_list = self.robot_list else: self.pre_robot_list = self.robot_list self._event_callback()
def _update(self, external_update=False): """ Logic for allocating resources after there has been a state change in either the list of available concert clients or the incoming resource requests. :param external_update bool: whether or not this is called inside the scheduler thread or not. :returns: list of requester id's that have notifications pending. We have to be careful with sending notifications if calling this from outside the scheduler thread. """ there_be_requests = False for request_set in self._request_sets.values(): if request_set.keys(): there_be_requests = True if not there_be_requests: return [] # nothing to do pending_notifications = [] ######################################## # Sort the request sets ######################################## unallocated_clients = [ client for client in self._clients.values() if not client.allocated ] new_replies = [] pending_replies = [] releasing_replies = [] for request_set in self._request_sets.values(): new_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.NEW ]) pending_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.WAITING ]) #pending_replies.extend([r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.NEW or r.msg.status == scheduler_msgs.Request.WAITING]) releasing_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.CANCELING ]) # get all requests for compatibility tree processing and sort by priority # this is a bit inefficient, should just sort the request set directly? modifying it directly may be not right though pending_replies[:] = sorted(pending_replies, key=lambda request: request.msg.priority) ######################################## # New ######################################## for reply in new_replies: reply.wait() ######################################## # Pending ######################################## resource_pool_state_changed = False # used to check if we should block further allocations to lower priorities # important to use this variable because we might have a group of requests with equal priorities # and we don't want to block the whole group because the first one failed. last_failed_priority = None reallocated_clients = { } # dic of uri string keys and request uuid hex strings for reply in pending_replies: request = reply.msg request_id = unique_id.toHexString(request.id) if last_failed_priority is not None and request.priority < last_failed_priority: rospy.loginfo( "Scheduler : ignoring lower priority requests until higher priorities are filled" ) break # add preemptible clients to the candidates if self._parameters['enable_preemptions']: allocatable_clients = unallocated_clients + [ client for client in self._clients.values() if (client.allocated and client.allocated_priority < request.priority) ] else: allocatable_clients = unallocated_clients if not allocatable_clients: # this gets spammy... #rospy.loginfo("Scheduler : no resources available to satisfy request [%s]" % request_id) last_failed_priority = request.priority continue compatibility_tree = create_compatibility_tree( request.resources, allocatable_clients) if self._parameters['debug_show_compatibility_tree']: compatibility_tree.print_branches("Compatibility Tree") pruned_branches = prune_compatibility_tree( compatibility_tree, verbosity=self._parameters['debug_show_compatibility_tree']) pruned_compatibility_tree = CompatibilityTree(pruned_branches) if self._parameters['debug_show_compatibility_tree']: pruned_compatibility_tree.print_branches("Pruned Tree", ' ') if pruned_compatibility_tree.is_valid(): rospy.loginfo( "Scheduler : compatibility tree is valid, attempting to allocate [%s]" % request_id) last_failed_priority = None resources = [] failed_to_allocate = False for branch in pruned_compatibility_tree.branches: if failed_to_allocate: # nested break catch break for leaf in branch.leaves: # there should be but one # this info is actually embedding into self._clients try: if leaf.allocated: old_request_id = leaf.reallocate( request_id, request.priority, branch.limb) reallocated_clients[leaf.msg.platform_info. uri] = old_request_id rospy.loginfo( "Scheduler : pre-empted and reallocated [%s]" % leaf.name) else: leaf.allocate(request_id, request.priority, branch.limb) rospy.loginfo("Scheduler : allocated [%s]" % leaf.name) except FailedToAllocateException as e: rospy.logwarn( "Scheduler : failed to (re)allocate [%s][%s]" % (leaf.name, str(e))) failed_to_allocate = True break resource = copy.deepcopy(branch.limb) uri = rocon_uri.parse( leaf.msg.platform_info.uri ) # leaf.msg is concert_msgs/ConcertClient uri.name = leaf.msg.gateway_name.lower().replace( ' ', '_') # store the unique name of the concert client resource.uri = str(uri) resources.append(resource) if failed_to_allocate: rospy.logwarn( "Scheduler : aborting request allocation [%s]" % request_id) # aborting request allocation for branch in pruned_compatibility_tree.branches: for leaf in branch.leaves: # there should be but one if leaf.allocated: leaf.abandon() last_failed_priority = request.priority else: reply.grant(resources) resource_pool_state_changed = True # remove allocated clients from the unallocated list so they don't get doubly allocated on the next request in line newly_allocated_client_names = [] for branch in pruned_compatibility_tree.branches: newly_allocated_client_names.extend([ leaf.name for leaf in branch.leaves if leaf.allocated ]) unallocated_clients[:] = [ client for client in unallocated_clients if client.name not in newly_allocated_client_names ] else: last_failed_priority = request.priority rospy.loginfo( "Scheduler : insufficient resources to satisfy request [%s]" % request_id) ######################################## # Preempted resource handling ######################################## # this is basically equivalent to what is in the concert client changes # subscriber callback...can we move this somewhere centralised? for (resource_uri_string, request_id_hexstring) in reallocated_clients.iteritems(): request_id = uuid.UUID(request_id_hexstring) # awkward that I don't have the requester id saved anywhere so # we have to parse through each requester's set of requests to find # the request id we want found = False for request_set in self._request_sets.values(): for request in request_set.values(): if request.uuid == request_id: for resource in request.msg.resources: # could use a better way to check for equality than this if resource.uri == resource_uri_string: updated_resource_uri = rocon_uri.parse( resource_uri_string) updated_resource_uri.name = concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE resource.uri = str(updated_resource_uri) found = True break if found: break if found: if external_update: pending_notifications.append(request_set.requester_id) else: self._scheduler.notify(request_set.requester_id) # @todo might want to consider changing the request status if all resources have been unallocated break ######################################## # Releasing ######################################## for reply in releasing_replies: if reply.msg.reason == scheduler_msgs.Request.NONE: rospy.loginfo( "Scheduler : releasing resources from cancelled request [%s][none]" % ([resource.rapp for resource in reply.msg.resources])) elif reply.msg.reason == scheduler_msgs.Request.TIMEOUT: rospy.loginfo( "Scheduler : releasing resources from cancelled request [%s][scheduler-requester watchdog timeout]" % ([resource.rapp for resource in reply.msg.resources])) else: rospy.loginfo( "Scheduler : releasing resources from cancelled request [%s][%s]" % ([resource.rapp for resource in reply.msg.resources], reply.msg.reason)) for resource in reply.msg.resources: try: resource_name = rocon_uri.parse(resource.uri).name.string for key, value in self._clients.items(): if resource_name == rocon_python_utils.ros.get_ros_friendly_name( key): value.abandon() except KeyError as e: rospy.logwarn( "Scheduler : Error while releasing resource[%s]" % str(e)) pass # nothing was allocated to that resource yet (i.e. unique gateway_name was not yet set) reply.close() #reply.msg.status = scheduler_msgs.Request.RELEASED # Publish an update? if resource_pool_state_changed or releasing_replies: self._publish_resource_pool() return pending_notifications
def _ros_subscriber_concert_client_changes(self, msg): """ Receives a list of all concert clients every time the state of the list changes (i.e. not periodically). @param msg : concert_msgs.ConcertClients """ pending_notifications = [] self._lock.acquire() invited_clients = msg.clients + msg.missing_clients # both connected and missing (lost wireless connection) # new_clients: concert_msgs.ConcertClient[] new_clients = [ client for client in invited_clients if client.gateway_name not in self._clients.keys() ] # lost_clients: common.ConcertClient[] lost_clients = [ client for client in self._clients.values() if client.gateway_name not in [c.gateway_name for c in invited_clients] ] # work over the client list for client in new_clients: rospy.loginfo("Scheduler : new concert client [%s]" % client.name) self._clients[client.gateway_name] = common.ConcertClient( client) # default setting is unallocated for client in lost_clients: if client.allocated: rospy.logwarn( "Scheduler : lost allocated concert client [%s]" % client.name) client_platform_uri = rocon_uri.parse( client.msg.platform_info.uri) client_platform_uri.name = client.msg.gateway_name for request_set in self._request_sets.values(): found = False for request in request_set.values(): for resource in request.msg.resources: # could use a better way to check for equality than this if resource.uri == str(client_platform_uri): client_platform_uri.name = concert_msgs.Strings.SCHEDULER_UNALLOCATED_RESOURCE resource.uri = str(client_platform_uri) found = True break if found: pending_notifications.append( request_set.requester_id) # @todo might want to consider changing the request status if all resources have been unallocated break if found: break # @todo might want to validate that we unallocated since we did detect an allocated flag del self._clients[client.gateway_name] if new_clients or lost_clients: self._publish_resource_pool() # should check for some things here, e.g. can we verify a client is allocated or not? pending_notifications.extend(self._update(external_update=True)) self._lock.release() # can't put this inside the lock scope since we can have the following deadlock scenario: # # 1) scheduler node's scheduler_requests subscriber callback thread # - scheduler is handling a scheduler_requests subscriber callback # - BIG LOCK.acquire() # - rqr.update() # - user callback # - CompatibilityTreeScheduler._update() # - self._lock.acquire() # 2) this conductor client subscriber callback thread # - self._lock.acquire() # - scheduler.notify() # - BIG LOCK.acquire() # # but does this have consequences? what if the update callback works on this request set # and publishes it and then we publish again here? for requester_id in pending_notifications: self._scheduler.notify(requester_id) # publishes the request set
def _update(self): """ Logic for allocating resources after there has been a state change in either the list of available concert clients or the incoming resource requests. Note : this must be protected by being called inside locked code @todo test use of rlocks here """ there_be_requests = False for request_set in self._request_sets.values(): if request_set.keys(): there_be_requests = True if not there_be_requests: return # Nothing to do ######################################## # Sort the request sets ######################################## unallocated_clients = [ client for client in self._clients.values() if not client.allocated ] new_replies = [] pending_replies = [] releasing_replies = [] for request_set in self._request_sets.values(): new_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.NEW ]) pending_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.NEW or r.msg.status == scheduler_msgs.Request.WAITING ]) releasing_replies.extend([ r for r in request_set.values() if r.msg.status == scheduler_msgs.Request.CANCELING ]) # get all requests for compatibility tree processing and sort by priority # this is a bit inefficient, should just sort the request set directly? modifying it directly may be not right though pending_replies[:] = sorted(pending_replies, key=lambda request: request.msg.priority) ######################################## # No allocations ######################################## if not unallocated_clients and new_replies: for reply in new_replies: # should do something here (maybe validation)? reply.wait() ######################################## # Allocations ######################################## resource_pool_state_changed = False if unallocated_clients: last_failed_priority = None # used to check if we should block further allocations to lower priorities for reply in pending_replies: request = reply.msg if last_failed_priority is not None and request.priority < last_failed_priority: rospy.loginfo( "Scheduler : ignoring lower priority requests until higher priorities are filled" ) break request_id = unique_id.toHexString(request.id) compatibility_tree = create_compatibility_tree( request.resources, unallocated_clients) if self._debug_show_compatibility_tree: compatibility_tree.print_branches("Compatibility Tree") pruned_branches = prune_compatibility_tree( compatibility_tree, verbosity=self._debug_show_compatibility_tree) pruned_compatibility_tree = CompatibilityTree(pruned_branches) if self._debug_show_compatibility_tree: pruned_compatibility_tree.print_branches( "Pruned Tree", ' ') if pruned_compatibility_tree.is_valid(): rospy.loginfo( "Scheduler : compatibility tree is valid, attempting to allocate [%s]" % request_id) last_failed_priority = None resources = [] failed_to_allocate = False for branch in pruned_compatibility_tree.branches: if failed_to_allocate: # nested break catch break for leaf in branch.leaves: # there should be but one # this info is actually embedding into self._clients try: leaf.allocate(request_id, branch.limb) rospy.loginfo("Scheduler : allocated [%s]" % leaf.name) except FailedToAllocateException as e: rospy.logwarn( "Scheduler : failed to allocate [%s][%s]" % (leaf.name, str(e))) failed_to_allocate = True break resource = copy.deepcopy(branch.limb) uri = rocon_uri.parse( leaf.msg.platform_info.uri ) # leaf.msg is concert_msgs/ConcertClient uri.name = leaf.msg.gateway_name # store the unique name of the concert client resource.uri = str(uri) resources.append(resource) if failed_to_allocate: rospy.logwarn( "Scheduler : aborting request allocation [%s]" % request_id) # aborting request allocation for branch in pruned_compatibility_tree.branches: for leaf in branch.leaves: # there should be but one if leaf.allocated: leaf.abandon() last_failed_priority = request.priority if reply.msg.status == scheduler_msgs.Request.NEW: reply.wait() else: reply.grant(resources) resource_pool_state_changed = True else: if reply.msg.status == scheduler_msgs.Request.NEW: reply.wait() last_failed_priority = request.priority rospy.loginfo( "Scheduler : insufficient resources to satisfy request [%s]" % request_id) ######################################## # Releasing Allocated Concert Clients ######################################## for reply in releasing_replies: #print("Releasing Resources: %s" % [r.rapp for r in reply.msg.resources]) #print(" Releasing Resources: %s" % [r.uri for r in reply.msg.resources]) #print(" Clients: %s" % self._clients.keys()) #for client in self._clients.values(): # print(str(client)) for resource in reply.msg.resources: try: self._clients[rocon_uri.parse( resource.uri).name.string].abandon() except KeyError: pass # nothing was allocated to that resource yet (i.e. unique gateway_name was not yet set) reply.close() #reply.msg.status = scheduler_msgs.Request.RELEASED # Publish an update? if resource_pool_state_changed or releasing_replies: self._publish_resource_pool()
self._remote_gateway_info_service.wait_for_service(0.5) except rospy.ROSException, e: self.cancel_pulls() raise ConcertClientException( "timed out on remote concert client services") except rospy.ServiceException, e: raise ConcertClientException(str(e)) self.cancel_pulls() # this call needs a timeout and also try/except block self.data.platform_info = platform_info_service().platform_info if self.data.platform_info.version != rocon_std_msgs.Strings.ROCON_VERSION: raise ConcertClientException( "concert client and conductor rocon versions do not match [%s][%s]" % (self.data.platform_info.version, rocon_std_msgs.Strings.ROCON_VERSION)) self.data.name = rocon_uri.parse( self.data.platform_info.uri).name.string # List Apps try: list_app_service.wait_for_service(0.5) except rospy.ROSException, e: self.cancel_pulls() raise ConcertClientException( "timed out on remote concert client 'list app' services") self.data.apps = list_app_service().available_apps def to_msg_format(self): ''' Returns the updated client information status in ros-consumable msg format. @return the client information as a ros message.