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.
