def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(
screen=bool(rospy.get_param('~use_screen', screen)))
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param(
'~missing_default_provider_is_an_error', False)
def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(screen=bool(rospy.get_param('~use_screen', screen)))
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param('~missing_default_provider_is_an_error', False)
class CapabilityServer(object):
"""A class to expose the :py:class:`discovery.SpecIndex` over a ROS API
"""
def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(
screen=bool(rospy.get_param('~use_screen', screen)),
nodelet_manager_name=rospy.get_param('~nodelet_manager_name', None)
)
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param('~missing_default_provider_is_an_error', False)
self.__bonds = {}
def spin(self):
"""Starts the capability server by setting up ROS comms, then spins"""
self.__package_whitelist = rospy.get_param('~package_whitelist', None)
if not isinstance(self.__package_whitelist, (list, tuple, type(None))):
msg = "~package_whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_whitelist = None
self.__package_blacklist = rospy.get_param('~package_blacklist', None)
if not isinstance(self.__package_blacklist, (list, tuple, type(None))):
msg = "~package_blacklist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_blacklist = None
self.__whitelist = rospy.get_param('~whitelist', None)
if not isinstance(self.__whitelist, (list, tuple, type(None))):
msg = "~whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__whitelist = None
self.__blacklist = rospy.get_param('~blacklist', None)
if not isinstance(self.__blacklist, (list, tuple, type(None))):
msg = "~blacklist must be a list or null, got a '{0}'".format(type(self.__blacklist))
rospy.logerr(msg)
self.__blacklist = None
self.__debug = rospy.get_param('~debug', False)
if self.__debug:
logger = logging.getLogger('rosout')
logger.setLevel(logging.DEBUG)
rospy.logdebug('Debug messages enabled.')
self.__load_capabilities()
self.__bond_topic = rospy.get_name() + "/bonds"
# Collect default arguments
self.__populate_default_providers()
rospy.Subscriber(
'~events', CapabilityEvent, self.handle_capability_events)
self.__start_capability_service = rospy.Service(
'~start_capability', StartCapability, self.handle_start_capability)
self.__stop_capability_service = rospy.Service(
'~stop_capability', StopCapability, self.handle_stop_capability)
self.__establish_bond_service = rospy.Service(
'~establish_bond', EstablishBond, self.handle_establish_bond)
self.__free_capability_service = rospy.Service(
'~free_capability', FreeCapability, self.handle_free_capability)
self.__use_capability_service = rospy.Service(
'~use_capability', UseCapability, self.handle_use_capability)
self.__reload_service = rospy.Service(
'~reload_capabilities', Empty, self.handle_reload_request)
self.__interfaces_service = rospy.Service(
'~get_interfaces', GetInterfaces, self.handle_get_interfaces)
self.__providers_service = rospy.Service(
'~get_providers', GetProviders, self.handle_get_providers)
self.__semantic_interfaces_service = rospy.Service(
'~get_semantic_interfaces', GetSemanticInterfaces,
self.handle_get_semantic_interfaces)
self.__running_capabilities = rospy.Service(
'~get_running_capabilities', GetRunningCapabilities,
self.handle_get_running_capabilities)
self.__capability_specs = rospy.Service(
'~get_capability_specs', GetCapabilitySpecs,
self.handle_get_capability_specs)
self.__capability_spec = rospy.Service(
'~get_capability_spec', GetCapabilitySpec,
self.handle_get_capability_spec)
self.__get_nodelet_manager_name_service = rospy.Service(
'~get_nodelet_manager_name', GetNodeletManagerName,
self.handle_get_nodelet_manager_name)
self.__get_remappings_service = rospy.Service(
'~get_remappings', GetRemappings,
self.handle_get_remappings)
rospy.loginfo("Capability Server Ready")
rospy.Publisher("~events", CapabilityEvent, queue_size=1000).publish(
CapabilityEvent(type=CapabilityEvent.SERVER_READY))
rospy.spin()
def shutdown(self):
"""Stops the capability server and cleans up any running processes"""
for instance in self.__capability_instances.values(): # pragma: no cover
if instance.state in ['running', 'launching']:
instance.stopped()
if instance.state == 'waiting':
instance.cancel()
self.__launch_manager.stop()
def __load_capabilities(self):
package_index = package_index_from_package_path(self.__package_paths)
self.spec_file_index = spec_file_index_from_package_index(package_index)
# Prune packages by black and white list
for package in self.spec_file_index.keys():
if self.__package_whitelist and package not in self.__package_whitelist:
rospy.loginfo("Package '{0}' not in whitelist, skipping.".format(package))
del self.spec_file_index[package]
elif self.__package_blacklist and package in self.__package_blacklist:
rospy.loginfo("Package '{0}' in blacklist, skipping.".format(package))
del self.spec_file_index[package]
# Generate spec_index from spec file index
spec_index, errors = spec_index_from_spec_file_index(self.spec_file_index)
if errors:
rospy.logerr("Errors were encountered loading capabilities:")
for error in errors:
rospy.logerr(" " + str(error.__class__.__name__) + ": " + str(error))
# Prune specific capabilities based on black and white lists
removed_interfaces = []
for specs, remove_func in [
(spec_index.interfaces, spec_index.remove_interface),
(spec_index.semantic_interfaces, spec_index.remove_semantic_interface),
(spec_index.providers, spec_index.remove_provider)
]:
for spec in specs.keys():
if self.__whitelist and spec not in self.__whitelist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is not in the whitelist, skipping.".format(spec))
elif self.__blacklist and spec in self.__blacklist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is in the blacklist, skipping.".format(spec))
# Remove providers which no longer have an interface
for interface in removed_interfaces:
for provider in spec_index.providers.values():
if provider.implements == interface:
spec_index.remove_provider(provider.name)
self.__spec_index = spec_index
# Prune spec_file_index
spec_paths = spec_index.interface_paths.values() + \
spec_index.semantic_interface_paths.values() + \
spec_index.provider_paths.values()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
package_dict[spec_type][:] = [path for path in package_dict[spec_type] if path in spec_paths]
def __populate_default_providers(self):
# Collect available interfaces
interfaces = self.__spec_index.interface_names + self.__spec_index.semantic_interface_names
for interface in interfaces:
# Collect the providers for each interface
providers = [n
for n, p in self.__spec_index.providers.items()
if p.implements == interface]
if not providers:
# If an interface has not providers, ignore it
rospy.logwarn("No providers for capability interface '{0}', not checking for default provider."
.format(interface))
continue
try:
# Try to get the default provider from the corresponding ros parameter
self.__default_providers[interface] = rospy.get_param('~defaults/' + interface)
except KeyError:
# No ros parameter set for this capability interface
rospy.logwarn("No default provider given for capability interface '{0}'. ".format(interface))
if len(providers) == 1:
# If there is only one provider, allow it to be the default
rospy.logwarn("'{0}' has only one provider, '{1}', using that as the default."
.format(interface, providers[0]))
self.__default_providers[interface] = providers[0]
else:
# Otherwise we can't decide
if self.__missing_default_provider_is_an_error:
rospy.logerr("Could not determine a default provider for capability interface '{0}', aborting."
.format(interface))
sys.exit(-1)
else:
rospy.logwarn("Could not determine a default provider for capability interface '{0}'."
.format(interface))
continue
# Make sure the given default provider exists
if self.__default_providers[interface] not in self.__spec_index.provider_names:
rospy.logerr("Given default provider '{0}' for interface '{1}' does not exist."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Make sure the given provider implements this interface
if self.__default_providers[interface] not in providers:
rospy.logerr("Given default provider '{0}' does not implment interface '{1}'."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Update the interface object with the default provider
iface = self.__spec_index.interfaces.get(
interface,
self.__spec_index.semantic_interfaces.get(interface, None))
iface.default_provider = self.__default_providers[interface]
# Summarize defaults
if self.__default_providers:
rospy.loginfo("For each available interface, the default provider:")
for interface, provider in self.__default_providers.items():
rospy.loginfo("'{0}'".format(interface))
rospy.loginfo(" => '{0}'".format(provider))
rospy.loginfo("")
else: # pragma: no cover
rospy.logwarn("No runnable Capabilities loaded.")
def __catch_and_log(self, func, *args, **kwargs):
warning_level_exceptions = ['because it is not running']
try:
return func(*args, **kwargs)
except Exception as exc:
msg = "{0}".format(exc)
log_func = rospy.logerr
if [x for x in warning_level_exceptions if x in msg]:
log_func = rospy.logwarn
rospy.logdebug(traceback.format_exc())
log_func('{0}: {1}'.format(exc.__class__.__name__, msg))
raise
def handle_capability_events(self, event):
"""Callback for handling messages (events) from the /events topic
This callback only process events generated by this node.
:param event: ROS message encapsulating an event
:type event: :py:class:`capabilities.msgs.CapabilityEvent`
"""
return self.__catch_and_log(self._handle_capability_events, event)
def _handle_capability_events(self, event):
# Ignore any publications which we did not send (external publishers)
if event._connection_header['callerid'] != rospy.get_name():
return # pragma: no cover
# Ignore the `server_ready` event
if event.type == event.SERVER_READY:
return
# Specially handle the nodelet manager
if event.capability == _special_nodelet_manager_capability:
if event.type == event.TERMINATED:
if not rospy.is_shutdown():
rospy.logerr("Capability server's nodelet manager terminated unexpectedly.")
self.shutdown()
return
# Update the capability
capability = event.capability
with self.__graph_lock:
if capability not in self.__capability_instances.keys():
rospy.logerr("Unknown capability instance: '{0}'"
.format(capability))
return
instance = self.__capability_instances[capability]
if event.type == event.LAUNCHED:
if instance.canceled: # pragma: no cover
# This is defensive programming, it should not happen
self.__stop_capability(instance.name)
else:
instance.launched(event.pid)
elif event.type == event.TERMINATED:
instance.terminated()
rospy.loginfo(
"Capability Provider '{0}' for Capability '{1}' "
.format(event.provider, event.capability) +
"has terminated.")
# Update the graph
self.__update_graph()
def __remove_terminated_capabilities(self):
# collect all of the terminated capabilities
terminated = [x
for x in self.__capability_instances.values()
if x.state == 'terminated']
# Remove terminated instances
for instance in terminated:
del self.__capability_instances[instance.interface]
# Shutdown unused capabilities
self.__cleanup_graph()
def __cleanup_graph(self):
"""Iterate over the running capabilities and shutdown ones which are no longer needed
For each running capability, if it was not started by the user then look at who depends on it.
If no other capabilities depend on it, then shut it down.
"""
# Collect all running capabilities
running_capabilities = [x
for x in self.__capability_instances.values()
if x.state == 'running']
for cap in running_capabilities:
if cap.started_by == USER_SERVICE_REASON:
# Started by user, do not garbage collect this
continue
rdepends = get_reverse_depends(cap.interface, self.__capability_instances.values())
if rdepends:
# Someone depends on me, do not garbage collect this
rospy.logdebug("Keeping the '{0}' provider of the '{1}' interface, ".format(cap.name, cap.interface) +
"because other running capabilities depend on it.")
continue
if cap.state == 'running':
rospy.loginfo("Stopping the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
self.__stop_capability(cap.interface)
elif cap.state == 'waiting': # pragma: no cover
rospy.loginfo("Canceling the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
cap.cancel()
# Else the state is launching, stopping, or terminated
# In which case launching will be caught on the next cleanup
# and the latter two will get cleared out also.
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [x
for x in self.__capability_instances.values()
if x.state == 'waiting']
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency_name in instance.depends_on:
if dependency_name not in self.__capability_instances: # pragma: no cover
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on "
.format(instance.name) + "'{0}', ".format(dependency_name) +
"which is not in the list of capability instances.")
return
dependency = self.__capability_instances[dependency_name]
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path
)
# Remove any terminated capabilities
self.__remove_terminated_capabilities()
def __stop_capability(self, name):
if name not in self.__capability_instances:
rospy.logerr("Inconsistent capability run graph, asked to stop " +
"capability '{0}', ".format(name) +
"which is not in the list of capability instances.")
return
capability = self.__capability_instances[name]
rdepends = get_reverse_depends(name, self.__capability_instances.values())
for cap in rdepends:
if cap.state in ['stopping', 'terminated']: # pragma: no cover
# It is possible that this cap was stopped by another cap in this list
# This is purely defensive
continue
rospy.loginfo(
"Capability '{0}' being stopped because its dependency '{1}' is being stopped.".format(cap.name, name))
self.__stop_capability(cap.interface)
capability.stopped()
self.__launch_manager.stop_capability_provider(capability.pid)
def __get_provider_dependencies(self, provider):
result = []
for interface, dep in provider.dependencies.items():
provider_name = dep.provider or self.__default_providers[interface]
if provider_name not in self.__spec_index.providers:
# This is the case where a provider depends on another interface,
# but the preferred provider does not exist
raise RuntimeError("Capability Provider '{0}' not found"
.format(provider_name))
dep_provider = self.__spec_index.providers[provider_name]
result.append((dep_provider, provider.name))
return result
def __get_capability_instances_from_provider(self, provider):
instances = []
providers = [(provider, USER_SERVICE_REASON)]
while providers:
curr, reason = providers.pop()
providers.extend(self.__get_provider_dependencies(curr))
curr_path = self.__spec_index.provider_paths[curr.name]
instances.append(CapabilityInstance(curr, curr_path, started_by=reason))
return instances
def __get_providers_for_interface(self, interface, allow_semantic=False):
valid_interfaces = [interface]
if allow_semantic:
# Add semantic interfaces which redefine this one
valid_interfaces.extend(
[k for k, v in self.__spec_index.semantic_interfaces.items()
if v.redefines == interface]
)
providers = dict([(n, p)
for n, p in self.__spec_index.providers.items()
if p.implements in valid_interfaces])
return providers # Could be empty
def __start_capability(self, capability, preferred_provider):
if capability not in self.__spec_index.interfaces.keys() + self.__spec_index.semantic_interfaces.keys():
raise RuntimeError("Capability '{0}' not found.".format(capability))
# If no preferred provider is given, use the default
preferred_provider = preferred_provider or self.__default_providers[capability]
providers = self.__get_providers_for_interface(capability, allow_semantic=True)
if preferred_provider not in providers:
raise RuntimeError(
"Capability Provider '{0}' not found for Capability '{1}'"
.format(preferred_provider, capability))
provider = providers[preferred_provider]
instances = self.__get_capability_instances_from_provider(provider)
with self.__graph_lock:
# If the requested capability has an existing instance, we don't start it
# again. Return a result that lets the callee know this happened.
requested_instance = instances[0]
if requested_instance.interface in self.__capability_instances:
requested_instance_state = self.__capability_instances[
requested_instance.interface].state
if requested_instance_state in ['running']:
# Current instance is running (or will be soon)
return StartCapabilityResponse.RESULT_CURRENTLY_RUNNING
elif requested_instance_state in ['waiting', 'launching']:
return StartCapabilityResponse.RESULT_CURRENTLY_STARTING
elif requested_instance_state in ['stopping', 'terminated']:
# Current instance is in the process of stopping
return StartCapabilityResponse.RESULT_CURRENTLY_STOPPING
else:
raise RuntimeError(
"Instance for capability '{0}' has improper state '{1}'"
.format(capability, requested_instance_state))
for x in instances:
if x.interface not in self.__capability_instances:
self.__capability_instances[x.interface] = x
self.__update_graph()
return StartCapabilityResponse.RESULT_SUCCESS
def handle_get_capability_specs(self, req):
return self.__catch_and_log(self._handle_get_capability_specs, req)
def _handle_get_capability_specs(self, req):
rospy.loginfo("Servicing request for capability specs...")
response = GetCapabilitySpecsResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
cs = CapabilitySpec(package_name, spec_type, raw, default_provider)
response.capability_specs.append(cs)
return response
def handle_get_capability_spec(self, req):
return self.__catch_and_log(self._handle_get_capability_spec, req)
def _handle_get_capability_spec(self, req):
rospy.loginfo("Servicing request for get capability spec '{0}'...".format(req.capability_spec))
response = GetCapabilitySpecResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
if iface and iface.name == req.capability_spec:
response.capability_spec = CapabilitySpec(package_name, spec_type, raw, default_provider)
return response
raise RuntimeError("Could not find requested spec '{0}'".format(req.capability_spec))
def handle_start_capability(self, req):
return self.__catch_and_log(self._handle_start_capability, req)
def _handle_start_capability(self, req):
msg = "Request to start capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
ret = self.__start_capability(req.capability, req.preferred_provider)
return StartCapabilityResponse(ret)
def handle_stop_capability(self, req):
return self.__catch_and_log(self._handle_stop_capability, req)
def _handle_stop_capability(self, req):
rospy.loginfo("Request to stop capability '{0}'".format(req.capability))
capability = req.capability
if capability not in self.__capability_instances:
raise RuntimeError("No Capability '{0}' running".format(capability))
self.__stop_capability(capability)
return StopCapabilityResponse(True)
def handle_establish_bond(self, req):
return self.__catch_and_log(self._handle_establish_bond, req)
def _handle_establish_bond(self, req):
rospy.loginfo("Request to establish a bond")
bond_id = str(uuid.uuid1())
def on_formed():
rospy.loginfo("Bond formed with bond_id of '{0}'"
.format(bond_id))
def on_broken():
# if bond_id in self.__bonds:
rospy.loginfo("Bond with bond id '{0}' was broken, freeing associated capabilities"
.format(bond_id))
self.__free_capabilities_by_bond_id(bond_id)
del self.__bonds[bond_id]
self.__bonds[bond_id] = Bond(self.__bond_topic, bond_id, on_broken=on_broken, on_formed=on_formed)
self.__bonds[bond_id].start()
return EstablishBondResponse(bond_id)
def __free_capabilities_by_bond_id(self, bond_id):
if bond_id in self.__bonds:
for capability in self.__capability_instances.values():
if bond_id in capability.bonds:
del capability.bonds[bond_id]
if capability.reference_count == 0:
rospy.loginfo("Capability '{0}' being stopped because it has zero references"
.format(capability.interface))
self.__stop_capability(capability.interface)
def __free_capability(self, capability_name, bond_id):
if capability_name not in self.__capability_instances:
# If you update this exception's message, then update the corresponding code
# in capabilities.client.CapabilitiesClient.free_capability()
raise RuntimeError("Cannot free Capability '{0}', because it is not running".format(capability_name))
capability = self.__capability_instances[capability_name]
if bond_id not in capability.bonds:
raise RuntimeError("Given bond_id '{0}' not associated with given capability '{1}'"
.format(bond_id, capability_name))
if capability.bonds[bond_id] == 0: # pragma: no cover
# this is defensive, it should never happen
raise RuntimeError("Cannot free capability '{0}' for bond_id '{1}', it already has a reference count of 0"
.format(capability_name, bond_id))
capability.bonds[bond_id] -= 1
if capability.reference_count == 0:
rospy.loginfo("Capability '{0}' being stopped because it has zero references"
.format(capability.interface))
self.__stop_capability(capability.interface)
def handle_free_capability(self, req):
return self.__catch_and_log(self._handle_free_capability, req)
def _handle_free_capability(self, req):
rospy.loginfo("Request to free usage of capability '{0}' (bond id '{1}')"
.format(req.capability, req.bond_id))
self.__free_capability(req.capability, req.bond_id)
return FreeCapabilityResponse()
def handle_use_capability(self, req):
return self.__catch_and_log(self._handle_use_capability, req)
def _handle_use_capability(self, req):
msg = "Request to use capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
# Make sure the bond_id is valid
if req.bond_id not in self.__bonds:
raise RuntimeError("Invalid bond_id given to ~use_capability: '{0}'".format(req.bond_id))
# Start the capability if it is not already running
if req.capability not in self.__capability_instances:
# This will raise if it failes to start the capability
self.__start_capability(req.capability, req.preferred_provider)
assert req.capability in self.__capability_instances # Should be true
# Get a handle ont he capability
capability = self.__capability_instances[req.capability]
if req.preferred_provider and capability.name != req.preferred_provider:
raise RuntimeError("Requested to use capability '{0}' with preferred provider '{1}', "
.format(capability.interface, req.preferred_provider) +
"but the capability is already running with provider '{0}'"
.format(capability.name))
if req.bond_id not in capability.bonds:
capability.bonds[req.bond_id] = 0
capability.bonds[req.bond_id] += 1
return UseCapabilityResponse()
def handle_reload_request(self, req):
return self.__catch_and_log(self._handle_reload_request, req)
def _handle_reload_request(self, req):
rospy.loginfo("Reloading capabilities...")
self.__load_capabilities()
return EmptyResponse()
def handle_get_interfaces(self, req):
return self.__catch_and_log(self._handle_get_interfaces, req)
def _handle_get_interfaces(self, req):
return GetInterfacesResponse(self.__spec_index.interface_names)
def handle_get_providers(self, req):
return self.__catch_and_log(self._handle_get_providers, req)
def _handle_get_providers(self, req):
if req.interface:
if req.interface not in self.__spec_index.interfaces.keys() + self.__spec_index.semantic_interfaces.keys():
raise RuntimeError("Capability Interface '{0}' not found.".format(req.interface))
providers = self.__get_providers_for_interface(req.interface, allow_semantic=req.include_semantic).keys()
default_provider = self.__default_providers[req.interface]
else:
providers = self.__spec_index.provider_names
default_provider = ''
return GetProvidersResponse(providers, default_provider)
def handle_get_semantic_interfaces(self, req):
return self.__catch_and_log(self._handle_get_semantic_interfaces, req)
def _handle_get_semantic_interfaces(self, req):
if req.interface:
sifaces = [si.name
for si in self.__spec_index.semantic_interfaces.values()
if si.redefines == req.interface]
else:
sifaces = self.__spec_index.semantic_interface_names
return GetSemanticInterfacesResponse(sifaces)
def handle_get_running_capabilities(self, req):
return self.__catch_and_log(self._handle_get_running_capabilities, req)
def _handle_get_running_capabilities(self, req):
resp = GetRunningCapabilitiesResponse()
for instance in self.__capability_instances.values():
if instance.state not in ['running']: # pragma: no cover
continue
running_capability = RunningCapability()
running_capability.capability = Capability(instance.interface, instance.name)
running_capability.started_by = instance.started_by
running_capability.pid = instance.pid
rdepends = get_reverse_depends(instance.interface, self.__capability_instances.values())
for dep in rdepends:
running_capability.dependent_capabilities.append(Capability(dep.interface, dep.name))
resp.running_capabilities.append(running_capability)
return resp
def handle_get_nodelet_manager_name(self, req):
return self.__catch_and_log(self._handle_get_nodelet_manager_name, req)
def _handle_get_nodelet_manager_name(self, req):
resp = GetNodeletManagerNameResponse()
resp.nodelet_manager_name = self.__launch_manager.nodelet_manager_name
return resp
def handle_get_remappings(self, req):
return self.__catch_and_log(self._handle_get_remappings, req)
def _handle_get_remappings(self, req):
interface = None
if req.spec in self.__capability_instances.keys():
interface = self.__capability_instances[req.spec]
else:
providers = dict([(i.provider.name, i) for i in self.__capability_instances.values()])
if req.spec not in providers:
raise RuntimeError("Spec '{0}' is neither a running Interface nor a running Provider."
.format(req.spec))
interface = providers[req.spec]
resp = GetRemappingsResponse()
remappings = {
'topics': {},
'services': {},
'actions': {},
'parameters': {}
}
# Iterate this instance and its recursive dependencies
for iface in reversed(self.__get_capability_instances_from_provider(interface.provider)):
# For each iterate over their remappings and add them to the combined remappings,
# flattening the remappings as you go
for map_type, mapping in iface.provider.remappings_by_type.items():
assert map_type in remappings
remappings[map_type].update(mapping)
# Collapse remapping chains
for mapping in remappings.values():
for key, value in mapping.items():
if value in mapping:
mapping[key] = mapping[value]
del mapping[value]
for map_type, mapping in remappings.items():
resp_mapping = getattr(resp, map_type)
for key, value in mapping.items():
remapping = Remapping()
remapping.key = key
remapping.value = value
resp_mapping.append(remapping)
return resp
class CapabilityServer(object):
"""A class to expose the :py:class:`discovery.SpecIndex` over a ROS API
"""
def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(screen=bool(rospy.get_param('~use_screen', screen)))
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param('~missing_default_provider_is_an_error', False)
def spin(self):
"""Starts the capability server by setting up ROS comms, then spins"""
self.__package_whitelist = rospy.get_param('~package_whitelist', None)
if not isinstance(self.__package_whitelist, (list, tuple, type(None))):
msg = "~package_whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_whitelist = None
self.__package_blacklist = rospy.get_param('~package_blacklist', None)
if not isinstance(self.__package_blacklist, (list, tuple, type(None))):
msg = "~package_blacklist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_blacklist = None
self.__whitelist = rospy.get_param('~whitelist', None)
if not isinstance(self.__whitelist, (list, tuple, type(None))):
msg = "~whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__whitelist = None
self.__blacklist = rospy.get_param('~blacklist', None)
if not isinstance(self.__blacklist, (list, tuple, type(None))):
msg = "~blacklist must be a list or null, got a '{0}'".format(type(self.__blacklist))
rospy.logerr(msg)
self.__blacklist = None
self.__debug = rospy.get_param('~debug', False)
if self.__debug:
logger = logging.getLogger('rosout')
logger.setLevel(logging.DEBUG)
rospy.logdebug('Debug messages enabled.')
self.__load_capabilities()
# Collect default arguments
self.__populate_default_providers()
self.__start_capability_service = rospy.Service(
'~start_capability', StartCapability, self.handle_start_capability)
self.__stop_capability_service = rospy.Service(
'~stop_capability', StopCapability, self.handle_stop_capability)
self.__reload_service = rospy.Service(
'~reload_capabilities', Empty, self.handle_reload_request)
self.__interfaces_service = rospy.Service(
'~get_interfaces', GetInterfaces, self.handle_get_interfaces)
self.__providers_service = rospy.Service(
'~get_providers', GetProviders, self.handle_get_providers)
self.__semantic_interfaces_service = rospy.Service(
'~get_semantic_interfaces', GetSemanticInterfaces,
self.handle_get_semantic_interfaces)
self.__running_capabilities = rospy.Service(
'~get_running_capabilities', GetRunningCapabilities,
self.handle_get_running_capabilities)
self.__capability_specs = rospy.Service(
'~get_capability_specs', GetCapabilitySpecs,
self.handle_get_capability_specs)
self.__capability_spec = rospy.Service(
'~get_capability_spec', GetCapabilitySpec,
self.handle_get_capability_spec)
rospy.Subscriber(
'~events', CapabilityEvent, self.handle_capability_events)
rospy.loginfo("Capability Server Ready")
rospy.Publisher("~events", CapabilityEvent).publish(CapabilityEvent(type=CapabilityEvent.SERVER_READY))
rospy.spin()
def shutdown(self):
"""Stops the capability server and cleans up any running processes"""
for instance in self.__capability_instances.values(): # pragma: no cover
if instance.state in ['running', 'launching']:
instance.stopped()
if instance.state == 'waiting':
instance.cancel()
self.__launch_manager.stop()
def __load_capabilities(self):
package_index = package_index_from_package_path(self.__package_paths)
self.spec_file_index = spec_file_index_from_package_index(package_index)
# Prune packages by black and white list
for package in self.spec_file_index.keys():
if self.__package_whitelist and package not in self.__package_whitelist:
rospy.loginfo("Package '{0}' not in whitelist, skipping.".format(package))
del self.spec_file_index[package]
if self.__package_blacklist and package in self.__package_blacklist:
rospy.loginfo("Package '{0}' in blacklist, skipping.".format(package))
del self.spec_file_index[package]
# Generate spec_index from spec file index
spec_index, errors = spec_index_from_spec_file_index(self.spec_file_index)
if errors:
rospy.logerr("Errors were encountered loading capabilities:")
for error in errors:
rospy.logerr(" " + str(error.__class__.__name__) + ": " + str(error))
# Prune specific capabilities based on black and white lists
removed_interfaces = []
for specs, remove_func in [
(spec_index.interfaces, spec_index.remove_interface),
(spec_index.semantic_interfaces, spec_index.remove_semantic_interface),
(spec_index.providers, spec_index.remove_provider)
]:
for spec in specs.keys():
if self.__whitelist and spec not in self.__whitelist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is not in the whitelist, skipping.".format(spec))
if self.__blacklist and spec in self.__blacklist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is in the blacklist, skipping.".format(spec))
# Remove providers which no longer have an interface
for interface in removed_interfaces:
for provider in spec_index.providers.values():
if provider.implements == interface:
spec_index.remove_provider(provider.name)
self.__spec_index = spec_index
def __populate_default_providers(self):
# Collect available interfaces
interfaces = self.__spec_index.interface_names + self.__spec_index.semantic_interface_names
for interface in interfaces:
# Collect the providers for each interface
providers = [n
for n, p in self.__spec_index.providers.items()
if p.implements == interface]
if not providers:
# If an interface has not providers, ignore it
rospy.logwarn("No providers for capability interface '{0}', not checking for default provider."
.format(interface))
continue
try:
# Try to get the default provider from the corresponding ros parameter
self.__default_providers[interface] = rospy.get_param('~defaults/' + interface)
except KeyError:
# No ros parameter set for this capability interface
rospy.logwarn("No default provider given for capability interface '{0}'. ".format(interface))
if len(providers) == 1:
# If there is only one provider, allow it to be the default
rospy.logwarn("'{0}' has only one provider, '{1}', using that as the default."
.format(interface, providers[0]))
self.__default_providers[interface] = providers[0]
else:
# Otherwise we can't decide
if self.__missing_default_provider_is_an_error:
rospy.logerr("Could not determine a default provider for capability interface '{0}', aborting."
.format(interface))
sys.exit(-1)
else:
rospy.logwarn("Could not determine a default provider for capability interface '{0}'."
.format(interface))
continue
# Make sure the given default provider exists
if self.__default_providers[interface] not in self.__spec_index.provider_names:
rospy.logerr("Given default provider '{0}' for interface '{1}' does not exist."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Make sure the given provider implements this interface
if self.__default_providers[interface] not in providers:
rospy.logerr("Given default provider '{0}' does not implment interface '{1}'."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Update the interface object with the default provider
iface = self.__spec_index.interfaces.get(
interface,
self.__spec_index.semantic_interfaces.get(interface, None))
iface.default_provider = self.__default_providers[interface]
# Summarize defaults
if self.__default_providers:
rospy.loginfo("For each available interface, the default provider:")
for interface, provider in self.__default_providers.items():
rospy.loginfo("'{0}'".format(interface))
rospy.loginfo(" => '{0}'".format(provider))
rospy.loginfo("")
else: # pragma: no cover
rospy.logwarn("No runnable Capabilities loaded.")
def __catch_and_log(self, func, *args, **kwargs):
try:
return func(*args, **kwargs)
except Exception as exc:
rospy.logdebug(traceback.format_exc())
rospy.logerr('{0}: {1}'.format(exc.__class__.__name__, str(exc)))
raise
def handle_capability_events(self, event):
"""Callback for handling messages (events) from the /events topic
This callback only process events generated by this node.
:param event: ROS message encapsulating an event
:type event: :py:class:`capabilities.msgs.CapabilityEvent`
"""
return self.__catch_and_log(self._handle_capability_events, event)
def _handle_capability_events(self, event):
# Ignore any publications which we did not send (external publishers)
if event._connection_header['callerid'] != rospy.get_name():
return # pragma: no cover
# Ignore the `server_ready` event
if event.type == event.SERVER_READY:
return
# Update the capability
capability = event.capability
with self.__graph_lock:
if capability not in self.__capability_instances.keys():
rospy.logerr("Unknown capability instance: '{0}'"
.format(capability))
return
instance = self.__capability_instances[capability]
if event.type == event.LAUNCHED:
if instance.canceled: # pragma: no cover
# This is defensive programming, it should not happen
self.__stop_capability(instance.name)
else:
instance.launched(event.pid)
elif event.type == event.TERMINATED:
instance.terminated()
rospy.loginfo(
"Capability Provider '{0}' for Capability '{1}' "
.format(event.provider, event.capability) +
"has terminated.")
# Update the graph
self.__update_graph()
def __remove_terminated_capabilities(self):
# collect all of the terminated capabilities
terminated = [x
for x in self.__capability_instances.values()
if x.state == 'terminated']
# Remove terminated instances
for instance in terminated:
del self.__capability_instances[instance.interface]
# Shutdown unused capabilities
self.__cleanup_graph()
def __cleanup_graph(self):
"""Iterate over the running capabilities and shutdown ones which are no longer needed
For each running capability, if it was not started by the user then look at who depends on it.
If no other capabilities depend on it, then shut it down.
"""
# Collect all running capabilities
running_capabilities = [x
for x in self.__capability_instances.values()
if x.state == 'running']
for cap in running_capabilities:
if cap.started_by == USER_SERVICE_REASON:
# Started by user, do not garbage collect this
continue
rdepends = get_reverse_depends(cap.interface, self.__capability_instances.values())
if rdepends:
# Someone depends on me, do not garbage collect this
rospy.logdebug("Keeping the '{0}' provider of the '{1}' interface, ".format(cap.name, cap.interface) +
"because other running capabilities depend on it.")
continue
if cap.state == 'running':
rospy.loginfo("Stopping the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
self.__stop_capability(cap.interface)
elif cap.state == 'waiting': # pragma: no cover
rospy.loginfo("Canceling the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
cap.cancel()
# Else the state is launching, stopping, or terminated
# In which case launching will be caught on the next cleanup
# and the latter two will get cleared out also.
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [x
for x in self.__capability_instances.values()
if x.state == 'waiting']
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency_name in instance.depends_on:
if dependency_name not in self.__capability_instances: # pragma: no cover
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on "
.format(instance.name) + "'{0}', ".format(dependency_name) +
"which is not in the list of capability instances.")
return
dependency = self.__capability_instances[dependency_name]
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path
)
# Remove any terminated capabilities
self.__remove_terminated_capabilities()
def __stop_capability(self, name):
if name not in self.__capability_instances:
rospy.logerr("Inconsistent capability run graph, asked to stop " +
"capability '{0}', ".format(name) +
"which is not in the list of capability instances.")
return
capability = self.__capability_instances[name]
rdepends = get_reverse_depends(name, self.__capability_instances.values())
for cap in rdepends:
self.__stop_capability(cap.interface)
capability.stopped()
self.__launch_manager.stop_capability_provider(capability.pid)
def __get_provider_dependencies(self, provider):
result = []
for interface, dep in provider.dependencies.items():
provider_name = dep.provider or self.__default_providers[interface]
if provider_name not in self.__spec_index.providers:
# This is the case where a provider depends on another interface,
# but the preferred provider does not exist
raise RuntimeError("Capability Provider '{0}' not found"
.format(provider_name))
dep_provider = self.__spec_index.providers[provider_name]
result.append((dep_provider, provider.name))
return result
def __get_capability_instances_from_provider(self, provider):
instances = []
providers = [(provider, USER_SERVICE_REASON)]
while providers:
curr, reason = providers.pop()
providers.extend(self.__get_provider_dependencies(curr))
curr_path = self.__spec_index.provider_paths[curr.name]
instances.append(CapabilityInstance(curr, curr_path, started_by=reason))
return instances
def __start_capability(self, capability, preferred_provider):
if capability not in self.__spec_index.interfaces.keys() + self.__spec_index.semantic_interfaces.keys():
raise RuntimeError("Capability '{0}' not found.".format(capability))
# If no preferred provider is given, use the default
preferred_provider = preferred_provider or self.__default_providers[capability]
providers = dict([(n, p)
for n, p in self.__spec_index.providers.items()
if p.implements == capability])
if preferred_provider not in providers:
raise RuntimeError(
"Capability Provider '{0}' not found for Capability '{1}'"
.format(preferred_provider, capability))
provider = providers[preferred_provider]
instances = self.__get_capability_instances_from_provider(provider)
with self.__graph_lock:
for x in instances:
if x.interface not in self.__capability_instances:
self.__capability_instances[x.interface] = x
self.__update_graph()
return True
def handle_get_capability_specs(self, req):
return self.__catch_and_log(self._handle_get_capability_specs, req)
def _handle_get_capability_specs(self, req):
rospy.loginfo("Servicing request for capability specs...")
response = GetCapabilitySpecsResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
cs = CapabilitySpec(package_name, spec_type, raw, default_provider)
response.capability_specs.append(cs)
return response
def handle_get_capability_spec(self, req):
return self.__catch_and_log(self._handle_get_capability_spec, req)
def _handle_get_capability_spec(self, req):
rospy.loginfo("Servicing request for get capability spec '{0}'...".format(req.capability_spec))
response = GetCapabilitySpecResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
if iface and iface.name == req.capability_spec:
response.capability_spec = CapabilitySpec(package_name, spec_type, raw, default_provider)
return response
raise RuntimeError("Could not find requested spec '{0}'".format(req.capability_spec))
def handle_start_capability(self, req):
return self.__catch_and_log(self._handle_start_capability, req)
def _handle_start_capability(self, req):
msg = "Request to start capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
ret = self.__start_capability(req.capability, req.preferred_provider)
return StartCapabilityResponse(ret or False)
def handle_stop_capability(self, req):
return self.__catch_and_log(self._handle_stop_capability, req)
def _handle_stop_capability(self, req):
rospy.loginfo("Request to stop capability '{0}'".format(req.capability))
capability = req.capability
if capability not in self.__capability_instances:
raise RuntimeError("No Capability '{0}' running".format(capability))
self.__stop_capability(capability)
return StopCapabilityResponse(True)
def handle_reload_request(self, req):
return self.__catch_and_log(self._handle_reload_request, req)
def _handle_reload_request(self, req):
rospy.loginfo("Reloading capabilities...")
self.__load_capabilities()
return EmptyResponse()
def handle_get_interfaces(self, req):
return self.__catch_and_log(self._handle_get_interfaces, req)
def _handle_get_interfaces(self, req):
return GetInterfacesResponse(self.__spec_index.interface_names)
def handle_get_providers(self, req):
return self.__catch_and_log(self._handle_get_providers, req)
def _handle_get_providers(self, req):
if req.interface:
providers = [n
for n, p in self.__spec_index.providers.items()
if p.implements == req.interface]
else:
providers = self.__spec_index.provider_names
return GetProvidersResponse(providers)
def handle_get_semantic_interfaces(self, req):
return self.__catch_and_log(self._handle_get_semantic_interfaces, req)
def _handle_get_semantic_interfaces(self, req):
if req.interface:
sifaces = [si.name
for si in self.__spec_index.semantic_interfaces.values()
if si.redefines == req.interface]
else:
sifaces = self.__spec_index.semantic_interface_names
return GetSemanticInterfacesResponse(sifaces)
def handle_get_running_capabilities(self, req):
return self.__catch_and_log(self._handle_get_running_capabilities, req)
def _handle_get_running_capabilities(self, req):
resp = GetRunningCapabilitiesResponse()
for instance in self.__capability_instances.values():
if instance.state not in ['running']: # pragma: no cover
continue
running_capability = RunningCapability()
running_capability.capability = Capability(instance.interface, instance.name)
running_capability.started_by = instance.started_by
running_capability.pid = instance.pid
rdepends = get_reverse_depends(instance.interface, self.__capability_instances.values())
for dep in rdepends:
running_capability.dependent_capabilities.append(Capability(dep.interface, dep.name))
resp.running_capabilities.append(running_capability)
return resp
class CapabilityServer(object):
"""A class to expose the :py:class:`discovery.SpecIndex` over a ROS API
"""
def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(
screen=bool(rospy.get_param('~use_screen', screen)),
nodelet_manager_name=rospy.get_param('~nodelet_manager_name', None)
)
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param('~missing_default_provider_is_an_error', False)
self.__bonds = {}
def spin(self):
"""Starts the capability server by setting up ROS comms, then spins"""
self.__package_whitelist = rospy.get_param('~package_whitelist', None)
if not isinstance(self.__package_whitelist, (list, tuple, type(None))):
msg = "~package_whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_whitelist = None
self.__package_blacklist = rospy.get_param('~package_blacklist', None)
if not isinstance(self.__package_blacklist, (list, tuple, type(None))):
msg = "~package_blacklist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__package_blacklist = None
self.__whitelist = rospy.get_param('~whitelist', None)
if not isinstance(self.__whitelist, (list, tuple, type(None))):
msg = "~whitelist must be a list or null, got a '{0}'".format(type(self.__whitelist))
rospy.logerr(msg)
self.__whitelist = None
self.__blacklist = rospy.get_param('~blacklist', None)
if not isinstance(self.__blacklist, (list, tuple, type(None))):
msg = "~blacklist must be a list or null, got a '{0}'".format(type(self.__blacklist))
rospy.logerr(msg)
self.__blacklist = None
self.__debug = rospy.get_param('~debug', False)
if self.__debug:
logger = logging.getLogger('rosout')
logger.setLevel(logging.DEBUG)
rospy.logdebug('Debug messages enabled.')
self.__load_capabilities()
self.__bond_topic = rospy.get_name() + "/bonds"
# Collect default arguments
self.__populate_default_providers()
rospy.Subscriber(
'~events', CapabilityEvent, self.handle_capability_events)
self.__start_capability_service = rospy.Service(
'~start_capability', StartCapability, self.handle_start_capability)
self.__stop_capability_service = rospy.Service(
'~stop_capability', StopCapability, self.handle_stop_capability)
self.__establish_bond_service = rospy.Service(
'~establish_bond', EstablishBond, self.handle_establish_bond)
self.__free_capability_service = rospy.Service(
'~free_capability', FreeCapability, self.handle_free_capability)
self.__use_capability_service = rospy.Service(
'~use_capability', UseCapability, self.handle_use_capability)
self.__reload_service = rospy.Service(
'~reload_capabilities', Empty, self.handle_reload_request)
self.__interfaces_service = rospy.Service(
'~get_interfaces', GetInterfaces, self.handle_get_interfaces)
self.__providers_service = rospy.Service(
'~get_providers', GetProviders, self.handle_get_providers)
self.__semantic_interfaces_service = rospy.Service(
'~get_semantic_interfaces', GetSemanticInterfaces,
self.handle_get_semantic_interfaces)
self.__running_capabilities = rospy.Service(
'~get_running_capabilities', GetRunningCapabilities,
self.handle_get_running_capabilities)
self.__capability_specs = rospy.Service(
'~get_capability_specs', GetCapabilitySpecs,
self.handle_get_capability_specs)
self.__capability_spec = rospy.Service(
'~get_capability_spec', GetCapabilitySpec,
self.handle_get_capability_spec)
self.__get_nodelet_manager_name_service = rospy.Service(
'~get_nodelet_manager_name', GetNodeletManagerName,
self.handle_get_nodelet_manager_name)
self.__get_remappings_service = rospy.Service(
'~get_remappings', GetRemappings,
self.handle_get_remappings)
rospy.loginfo("Capability Server Ready")
rospy.Publisher("~events", CapabilityEvent, queue_size=1000).publish(
CapabilityEvent(type=CapabilityEvent.SERVER_READY))
rospy.spin()
def shutdown(self):
"""Stops the capability server and cleans up any running processes"""
for instance in self.__capability_instances.values(): # pragma: no cover
if instance.state in ['running', 'launching']:
instance.stopped()
if instance.state == 'waiting':
instance.cancel()
self.__launch_manager.stop()
def __load_capabilities(self):
package_index = package_index_from_package_path(self.__package_paths)
self.spec_file_index = spec_file_index_from_package_index(package_index)
# Prune packages by black and white list
for package in self.spec_file_index.keys():
if self.__package_whitelist and package not in self.__package_whitelist:
rospy.loginfo("Package '{0}' not in whitelist, skipping.".format(package))
del self.spec_file_index[package]
elif self.__package_blacklist and package in self.__package_blacklist:
rospy.loginfo("Package '{0}' in blacklist, skipping.".format(package))
del self.spec_file_index[package]
# Generate spec_index from spec file index
spec_index, errors = spec_index_from_spec_file_index(self.spec_file_index)
if errors:
rospy.logerr("Errors were encountered loading capabilities:")
for error in errors:
rospy.logerr(" " + str(error.__class__.__name__) + ": " + str(error))
# Prune specific capabilities based on black and white lists
removed_interfaces = []
for specs, remove_func in [
(spec_index.interfaces, spec_index.remove_interface),
(spec_index.semantic_interfaces, spec_index.remove_semantic_interface),
(spec_index.providers, spec_index.remove_provider)
]:
for spec in specs.keys():
if self.__whitelist and spec not in self.__whitelist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is not in the whitelist, skipping.".format(spec))
elif self.__blacklist and spec in self.__blacklist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo("Spec '{0}' is in the blacklist, skipping.".format(spec))
# Remove providers which no longer have an interface
for interface in removed_interfaces:
for provider in spec_index.providers.values():
if provider.implements == interface:
spec_index.remove_provider(provider.name)
self.__spec_index = spec_index
# Prune spec_file_index
spec_paths = list(spec_index.interface_paths.values()) + \
list(spec_index.semantic_interface_paths.values()) + \
list(spec_index.provider_paths.values())
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
package_dict[spec_type][:] = [path for path in package_dict[spec_type] if path in spec_paths]
def __populate_default_providers(self):
# Collect available interfaces
interfaces = self.__spec_index.interface_names + self.__spec_index.semantic_interface_names
for interface in interfaces:
# Collect the providers for each interface
providers = [n
for n, p in self.__spec_index.providers.items()
if p.implements == interface]
if not providers:
# If an interface has not providers, ignore it
rospy.logwarn("No providers for capability interface '{0}', not checking for default provider."
.format(interface))
continue
try:
# Try to get the default provider from the corresponding ros parameter
self.__default_providers[interface] = rospy.get_param('~defaults/' + interface)
except KeyError:
# No ros parameter set for this capability interface
rospy.logwarn("No default provider given for capability interface '{0}'. ".format(interface))
if len(providers) == 1:
# If there is only one provider, allow it to be the default
rospy.logwarn("'{0}' has only one provider, '{1}', using that as the default."
.format(interface, providers[0]))
self.__default_providers[interface] = providers[0]
else:
# Otherwise we can't decide
if self.__missing_default_provider_is_an_error:
rospy.logerr("Could not determine a default provider for capability interface '{0}', aborting."
.format(interface))
sys.exit(-1)
else:
rospy.logwarn("Could not determine a default provider for capability interface '{0}'."
.format(interface))
continue
# Make sure the given default provider exists
if self.__default_providers[interface] not in self.__spec_index.provider_names:
rospy.logerr("Given default provider '{0}' for interface '{1}' does not exist."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Make sure the given provider implements this interface
if self.__default_providers[interface] not in providers:
rospy.logerr("Given default provider '{0}' does not implment interface '{1}'."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Update the interface object with the default provider
iface = self.__spec_index.interfaces.get(
interface,
self.__spec_index.semantic_interfaces.get(interface, None))
iface.default_provider = self.__default_providers[interface]
# Summarize defaults
if self.__default_providers:
rospy.loginfo("For each available interface, the default provider:")
for interface, provider in self.__default_providers.items():
rospy.loginfo("'{0}'".format(interface))
rospy.loginfo(" => '{0}'".format(provider))
rospy.loginfo("")
else: # pragma: no cover
rospy.logwarn("No runnable Capabilities loaded.")
def __catch_and_log(self, func, *args, **kwargs):
warning_level_exceptions = ['because it is not running']
try:
return func(*args, **kwargs)
except Exception as exc:
msg = "{0}".format(exc)
log_func = rospy.logerr
if [x for x in warning_level_exceptions if x in msg]:
log_func = rospy.logwarn
rospy.logdebug(traceback.format_exc())
log_func('{0}: {1}'.format(exc.__class__.__name__, msg))
raise
def handle_capability_events(self, event):
"""Callback for handling messages (events) from the /events topic
This callback only process events generated by this node.
:param event: ROS message encapsulating an event
:type event: :py:class:`capabilities.msgs.CapabilityEvent`
"""
return self.__catch_and_log(self._handle_capability_events, event)
def _handle_capability_events(self, event):
# Ignore any publications which we did not send (external publishers)
if event._connection_header['callerid'] != rospy.get_name():
return # pragma: no cover
# Ignore the `server_ready` event
if event.type == event.SERVER_READY:
return
# Specially handle the nodelet manager
if event.capability == _special_nodelet_manager_capability:
if event.type == event.TERMINATED:
if not rospy.is_shutdown():
rospy.logerr("Capability server's nodelet manager terminated unexpectedly.")
self.shutdown()
return
# Update the capability
capability = event.capability
with self.__graph_lock:
if capability not in self.__capability_instances.keys():
rospy.logerr("Unknown capability instance: '{0}'"
.format(capability))
return
instance = self.__capability_instances[capability]
if event.type == event.LAUNCHED:
if instance.canceled: # pragma: no cover
# This is defensive programming, it should not happen
self.__stop_capability(instance.name)
else:
instance.launched(event.pid)
elif event.type == event.TERMINATED:
instance.terminated()
rospy.loginfo(
"Capability Provider '{0}' for Capability '{1}' "
.format(event.provider, event.capability) +
"has terminated.")
# Update the graph
self.__update_graph()
def __remove_terminated_capabilities(self):
# collect all of the terminated capabilities
terminated = [x
for x in self.__capability_instances.values()
if x.state == 'terminated']
# Remove terminated instances
for instance in terminated:
del self.__capability_instances[instance.interface]
# Shutdown unused capabilities
self.__cleanup_graph()
def __cleanup_graph(self):
"""Iterate over the running capabilities and shutdown ones which are no longer needed
For each running capability, if it was not started by the user then look at who depends on it.
If no other capabilities depend on it, then shut it down.
"""
# Collect all running capabilities
running_capabilities = [x
for x in self.__capability_instances.values()
if x.state == 'running']
for cap in running_capabilities:
if cap.started_by == USER_SERVICE_REASON:
# Started by user, do not garbage collect this
continue
rdepends = get_reverse_depends(cap.interface, self.__capability_instances.values())
if rdepends:
# Someone depends on me, do not garbage collect this
rospy.logdebug("Keeping the '{0}' provider of the '{1}' interface, ".format(cap.name, cap.interface) +
"because other running capabilities depend on it.")
continue
if cap.state == 'running':
rospy.loginfo("Stopping the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
self.__stop_capability(cap.interface)
elif cap.state == 'waiting': # pragma: no cover
rospy.loginfo("Canceling the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
cap.cancel()
# Else the state is launching, stopping, or terminated
# In which case launching will be caught on the next cleanup
# and the latter two will get cleared out also.
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [x
for x in self.__capability_instances.values()
if x.state == 'waiting']
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency_name in instance.depends_on:
if dependency_name not in self.__capability_instances: # pragma: no cover
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on "
.format(instance.name) + "'{0}', ".format(dependency_name) +
"which is not in the list of capability instances.")
return
dependency = self.__capability_instances[dependency_name]
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path
)
# Remove any terminated capabilities
self.__remove_terminated_capabilities()
def __stop_capability(self, name):
if name not in self.__capability_instances:
rospy.logerr("Inconsistent capability run graph, asked to stop " +
"capability '{0}', ".format(name) +
"which is not in the list of capability instances.")
return
capability = self.__capability_instances[name]
rdepends = get_reverse_depends(name, self.__capability_instances.values())
for cap in rdepends:
if cap.state in ['stopping', 'terminated']: # pragma: no cover
# It is possible that this cap was stopped by another cap in this list
# This is purely defensive
continue
rospy.loginfo(
"Capability '{0}' being stopped because its dependency '{1}' is being stopped.".format(cap.name, name))
self.__stop_capability(cap.interface)
capability.stopped()
self.__launch_manager.stop_capability_provider(capability.pid)
def __get_provider_dependencies(self, provider):
result = []
for interface, dep in provider.dependencies.items():
provider_name = dep.provider or self.__default_providers[interface]
if provider_name not in self.__spec_index.providers:
# This is the case where a provider depends on another interface,
# but the preferred provider does not exist
raise RuntimeError("Capability Provider '{0}' not found"
.format(provider_name))
dep_provider = self.__spec_index.providers[provider_name]
result.append((dep_provider, provider.name))
return result
def __get_capability_instances_from_provider(self, provider):
instances = []
providers = [(provider, USER_SERVICE_REASON)]
while providers:
curr, reason = providers.pop()
providers.extend(self.__get_provider_dependencies(curr))
curr_path = self.__spec_index.provider_paths[curr.name]
instances.append(CapabilityInstance(curr, curr_path, started_by=reason))
return instances
def __get_providers_for_interface(self, interface, allow_semantic=False):
valid_interfaces = [interface]
if allow_semantic:
# Add semantic interfaces which redefine this one
valid_interfaces.extend(
[k for k, v in self.__spec_index.semantic_interfaces.items()
if v.redefines == interface]
)
providers = dict([(n, p)
for n, p in self.__spec_index.providers.items()
if p.implements in valid_interfaces])
return providers # Could be empty
def __start_capability(self, capability, preferred_provider):
if capability not in list(self.__spec_index.interfaces.keys()) + list(self.__spec_index.semantic_interfaces.keys()):
raise RuntimeError("Capability '{0}' not found.".format(capability))
# If no preferred provider is given, use the default
preferred_provider = preferred_provider or self.__default_providers[capability]
providers = self.__get_providers_for_interface(capability, allow_semantic=True)
if preferred_provider not in providers:
raise RuntimeError(
"Capability Provider '{0}' not found for Capability '{1}'"
.format(preferred_provider, capability))
provider = providers[preferred_provider]
instances = self.__get_capability_instances_from_provider(provider)
with self.__graph_lock:
# If the requested capability has an existing instance, we don't start it
# again. Return a result that lets the callee know this happened.
requested_instance = instances[0]
if requested_instance.interface in self.__capability_instances:
requested_instance_state = self.__capability_instances[
requested_instance.interface].state
if requested_instance_state in ['running']:
# Current instance is running (or will be soon)
return StartCapabilityResponse.RESULT_CURRENTLY_RUNNING
elif requested_instance_state in ['waiting', 'launching']:
return StartCapabilityResponse.RESULT_CURRENTLY_STARTING
elif requested_instance_state in ['stopping', 'terminated']:
# Current instance is in the process of stopping
return StartCapabilityResponse.RESULT_CURRENTLY_STOPPING
else:
raise RuntimeError(
"Instance for capability '{0}' has improper state '{1}'"
.format(capability, requested_instance_state))
for x in instances:
if x.interface not in self.__capability_instances:
self.__capability_instances[x.interface] = x
self.__update_graph()
return StartCapabilityResponse.RESULT_SUCCESS
def handle_get_capability_specs(self, req):
return self.__catch_and_log(self._handle_get_capability_specs, req)
def _handle_get_capability_specs(self, req):
rospy.loginfo("Servicing request for capability specs...")
response = GetCapabilitySpecsResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
cs = CapabilitySpec(package_name, spec_type, raw, default_provider)
response.capability_specs.append(cs)
return response
def handle_get_capability_spec(self, req):
return self.__catch_and_log(self._handle_get_capability_spec, req)
def _handle_get_capability_spec(self, req):
rospy.loginfo("Servicing request for get capability spec '{0}'...".format(req.capability_spec))
response = GetCapabilitySpecResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in ['capability_interface', 'semantic_capability_interface', 'capability_provider']:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(raw, path)
if spec_type in ['capability_interface', 'semantic_capability_interface']:
iface.name = '{package}/{name}'.format(package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[iface.name]
if iface and iface.name == req.capability_spec:
response.capability_spec = CapabilitySpec(package_name, spec_type, raw, default_provider)
return response
raise RuntimeError("Could not find requested spec '{0}'".format(req.capability_spec))
def handle_start_capability(self, req):
return self.__catch_and_log(self._handle_start_capability, req)
def _handle_start_capability(self, req):
msg = "Request to start capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
ret = self.__start_capability(req.capability, req.preferred_provider)
return StartCapabilityResponse(ret)
def handle_stop_capability(self, req):
return self.__catch_and_log(self._handle_stop_capability, req)
def _handle_stop_capability(self, req):
rospy.loginfo("Request to stop capability '{0}'".format(req.capability))
capability = req.capability
if capability not in self.__capability_instances:
raise RuntimeError("No Capability '{0}' running".format(capability))
self.__stop_capability(capability)
return StopCapabilityResponse(True)
def handle_establish_bond(self, req):
return self.__catch_and_log(self._handle_establish_bond, req)
def _handle_establish_bond(self, req):
rospy.loginfo("Request to establish a bond")
bond_id = str(uuid.uuid1())
def on_formed():
rospy.loginfo("Bond formed with bond_id of '{0}'"
.format(bond_id))
def on_broken():
# if bond_id in self.__bonds:
rospy.loginfo("Bond with bond id '{0}' was broken, freeing associated capabilities"
.format(bond_id))
self.__free_capabilities_by_bond_id(bond_id)
del self.__bonds[bond_id]
self.__bonds[bond_id] = Bond(self.__bond_topic, bond_id, on_broken=on_broken, on_formed=on_formed)
self.__bonds[bond_id].start()
return EstablishBondResponse(bond_id)
def __free_capabilities_by_bond_id(self, bond_id):
if bond_id in self.__bonds:
for capability in self.__capability_instances.values():
if bond_id in capability.bonds:
del capability.bonds[bond_id]
if capability.reference_count == 0:
rospy.loginfo("Capability '{0}' being stopped because it has zero references"
.format(capability.interface))
self.__stop_capability(capability.interface)
def __free_capability(self, capability_name, bond_id):
if capability_name not in self.__capability_instances:
# If you update this exception's message, then update the corresponding code
# in capabilities.client.CapabilitiesClient.free_capability()
raise RuntimeError("Cannot free Capability '{0}', because it is not running".format(capability_name))
capability = self.__capability_instances[capability_name]
if bond_id not in capability.bonds:
raise RuntimeError("Given bond_id '{0}' not associated with given capability '{1}'"
.format(bond_id, capability_name))
if capability.bonds[bond_id] == 0: # pragma: no cover
# this is defensive, it should never happen
raise RuntimeError("Cannot free capability '{0}' for bond_id '{1}', it already has a reference count of 0"
.format(capability_name, bond_id))
capability.bonds[bond_id] -= 1
if capability.reference_count == 0:
rospy.loginfo("Capability '{0}' being stopped because it has zero references"
.format(capability.interface))
self.__stop_capability(capability.interface)
def handle_free_capability(self, req):
return self.__catch_and_log(self._handle_free_capability, req)
def _handle_free_capability(self, req):
rospy.loginfo("Request to free usage of capability '{0}' (bond id '{1}')"
.format(req.capability, req.bond_id))
self.__free_capability(req.capability, req.bond_id)
return FreeCapabilityResponse()
def handle_use_capability(self, req):
return self.__catch_and_log(self._handle_use_capability, req)
def _handle_use_capability(self, req):
msg = "Request to use capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
# Make sure the bond_id is valid
if req.bond_id not in self.__bonds:
raise RuntimeError("Invalid bond_id given to ~use_capability: '{0}'".format(req.bond_id))
# Start the capability if it is not already running
if req.capability not in self.__capability_instances:
# This will raise if it failes to start the capability
self.__start_capability(req.capability, req.preferred_provider)
assert req.capability in self.__capability_instances # Should be true
# Get a handle ont he capability
capability = self.__capability_instances[req.capability]
if req.preferred_provider and capability.name != req.preferred_provider:
raise RuntimeError("Requested to use capability '{0}' with preferred provider '{1}', "
.format(capability.interface, req.preferred_provider) +
"but the capability is already running with provider '{0}'"
.format(capability.name))
if req.bond_id not in capability.bonds:
capability.bonds[req.bond_id] = 0
capability.bonds[req.bond_id] += 1
return UseCapabilityResponse()
def handle_reload_request(self, req):
return self.__catch_and_log(self._handle_reload_request, req)
def _handle_reload_request(self, req):
rospy.loginfo("Reloading capabilities...")
self.__load_capabilities()
return EmptyResponse()
def handle_get_interfaces(self, req):
return self.__catch_and_log(self._handle_get_interfaces, req)
def _handle_get_interfaces(self, req):
return GetInterfacesResponse(self.__spec_index.interface_names)
def handle_get_providers(self, req):
return self.__catch_and_log(self._handle_get_providers, req)
def _handle_get_providers(self, req):
if req.interface:
if req.interface not in self.__spec_index.interfaces.keys() + self.__spec_index.semantic_interfaces.keys():
raise RuntimeError("Capability Interface '{0}' not found.".format(req.interface))
providers = self.__get_providers_for_interface(req.interface, allow_semantic=req.include_semantic).keys()
default_provider = self.__default_providers[req.interface]
else:
providers = self.__spec_index.provider_names
default_provider = ''
return GetProvidersResponse(providers, default_provider)
def handle_get_semantic_interfaces(self, req):
return self.__catch_and_log(self._handle_get_semantic_interfaces, req)
def _handle_get_semantic_interfaces(self, req):
if req.interface:
sifaces = [si.name
for si in self.__spec_index.semantic_interfaces.values()
if si.redefines == req.interface]
else:
sifaces = self.__spec_index.semantic_interface_names
return GetSemanticInterfacesResponse(sifaces)
def handle_get_running_capabilities(self, req):
return self.__catch_and_log(self._handle_get_running_capabilities, req)
def _handle_get_running_capabilities(self, req):
resp = GetRunningCapabilitiesResponse()
for instance in self.__capability_instances.values():
if instance.state not in ['running']: # pragma: no cover
continue
running_capability = RunningCapability()
running_capability.capability = Capability(instance.interface, instance.name)
running_capability.started_by = instance.started_by
running_capability.pid = instance.pid
rdepends = get_reverse_depends(instance.interface, self.__capability_instances.values())
for dep in rdepends:
running_capability.dependent_capabilities.append(Capability(dep.interface, dep.name))
resp.running_capabilities.append(running_capability)
return resp
def handle_get_nodelet_manager_name(self, req):
return self.__catch_and_log(self._handle_get_nodelet_manager_name, req)
def _handle_get_nodelet_manager_name(self, req):
resp = GetNodeletManagerNameResponse()
resp.nodelet_manager_name = self.__launch_manager.nodelet_manager_name
return resp
def handle_get_remappings(self, req):
return self.__catch_and_log(self._handle_get_remappings, req)
def _handle_get_remappings(self, req):
interface = None
if req.spec in self.__capability_instances.keys():
interface = self.__capability_instances[req.spec]
else:
providers = dict([(i.provider.name, i) for i in self.__capability_instances.values()])
if req.spec not in providers:
raise RuntimeError("Spec '{0}' is neither a running Interface nor a running Provider."
.format(req.spec))
interface = providers[req.spec]
resp = GetRemappingsResponse()
remappings = {
'topics': {},
'services': {},
'actions': {},
'parameters': {}
}
# Iterate this instance and its recursive dependencies
for iface in reversed(self.__get_capability_instances_from_provider(interface.provider)):
# For each iterate over their remappings and add them to the combined remappings,
# flattening the remappings as you go
for map_type, mapping in iface.provider.remappings_by_type.items():
assert map_type in remappings
remappings[map_type].update(mapping)
# Collapse remapping chains
for mapping in remappings.values():
for key, value in mapping.items():
if value in mapping:
mapping[key] = mapping[value]
del mapping[value]
for map_type, mapping in remappings.items():
resp_mapping = getattr(resp, map_type)
for key, value in mapping.items():
remapping = Remapping()
remapping.key = key
remapping.value = value
resp_mapping.append(remapping)
return resp
class CapabilityServer(object):
"""A class to expose the :py:class:`discovery.SpecIndex` over a ROS API
"""
def __init__(self, package_paths, screen=None):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager(
screen=bool(rospy.get_param('~use_screen', screen)))
self.__debug = False
self.__package_whitelist = None
self.__package_blacklist = None
self.__whitelist = None
self.__blacklist = None
self.__default_providers = {}
self.__missing_default_provider_is_an_error = rospy.get_param(
'~missing_default_provider_is_an_error', False)
def spin(self):
"""Starts the capability server by setting up ROS comms, then spins"""
self.__package_whitelist = rospy.get_param('~package_whitelist', None)
if not isinstance(self.__package_whitelist, (list, tuple, type(None))):
msg = "~package_whitelist must be a list or null, got a '{0}'".format(
type(self.__whitelist))
rospy.logerr(msg)
self.__package_whitelist = None
self.__package_blacklist = rospy.get_param('~package_blacklist', None)
if not isinstance(self.__package_blacklist, (list, tuple, type(None))):
msg = "~package_blacklist must be a list or null, got a '{0}'".format(
type(self.__whitelist))
rospy.logerr(msg)
self.__package_blacklist = None
self.__whitelist = rospy.get_param('~whitelist', None)
if not isinstance(self.__whitelist, (list, tuple, type(None))):
msg = "~whitelist must be a list or null, got a '{0}'".format(
type(self.__whitelist))
rospy.logerr(msg)
self.__whitelist = None
self.__blacklist = rospy.get_param('~blacklist', None)
if not isinstance(self.__blacklist, (list, tuple, type(None))):
msg = "~blacklist must be a list or null, got a '{0}'".format(
type(self.__blacklist))
rospy.logerr(msg)
self.__blacklist = None
self.__debug = rospy.get_param('~debug', False)
if self.__debug:
logger = logging.getLogger('rosout')
logger.setLevel(logging.DEBUG)
rospy.logdebug('Debug messages enabled.')
self.__load_capabilities()
# Collect default arguments
self.__populate_default_providers()
self.__start_capability_service = rospy.Service(
'~start_capability', StartCapability, self.handle_start_capability)
self.__stop_capability_service = rospy.Service(
'~stop_capability', StopCapability, self.handle_stop_capability)
self.__reload_service = rospy.Service('~reload_capabilities', Empty,
self.handle_reload_request)
self.__interfaces_service = rospy.Service('~get_interfaces',
GetInterfaces,
self.handle_get_interfaces)
self.__providers_service = rospy.Service('~get_providers',
GetProviders,
self.handle_get_providers)
self.__semantic_interfaces_service = rospy.Service(
'~get_semantic_interfaces', GetSemanticInterfaces,
self.handle_get_semantic_interfaces)
self.__running_capabilities = rospy.Service(
'~get_running_capabilities', GetRunningCapabilities,
self.handle_get_running_capabilities)
self.__capability_specs = rospy.Service(
'~get_capability_specs', GetCapabilitySpecs,
self.handle_get_capability_specs)
self.__capability_spec = rospy.Service('~get_capability_spec',
GetCapabilitySpec,
self.handle_get_capability_spec)
rospy.Subscriber('~events', CapabilityEvent,
self.handle_capability_events)
rospy.loginfo("Capability Server Ready")
rospy.Publisher("~events", CapabilityEvent).publish(
CapabilityEvent(type=CapabilityEvent.SERVER_READY))
rospy.spin()
def shutdown(self):
"""Stops the capability server and cleans up any running processes"""
for instance in self.__capability_instances.values(
): # pragma: no cover
if instance.state in ['running', 'launching']:
instance.stopped()
if instance.state == 'waiting':
instance.cancel()
self.__launch_manager.stop()
def __load_capabilities(self):
package_index = package_index_from_package_path(self.__package_paths)
self.spec_file_index = spec_file_index_from_package_index(
package_index)
# Prune packages by black and white list
for package in self.spec_file_index.keys():
if self.__package_whitelist and package not in self.__package_whitelist:
rospy.loginfo(
"Package '{0}' not in whitelist, skipping.".format(
package))
del self.spec_file_index[package]
if self.__package_blacklist and package in self.__package_blacklist:
rospy.loginfo(
"Package '{0}' in blacklist, skipping.".format(package))
del self.spec_file_index[package]
# Generate spec_index from spec file index
spec_index, errors = spec_index_from_spec_file_index(
self.spec_file_index)
if errors:
rospy.logerr("Errors were encountered loading capabilities:")
for error in errors:
rospy.logerr(" " + str(error.__class__.__name__) + ": " +
str(error))
# Prune specific capabilities based on black and white lists
removed_interfaces = []
for specs, remove_func in [
(spec_index.interfaces, spec_index.remove_interface),
(spec_index.semantic_interfaces,
spec_index.remove_semantic_interface),
(spec_index.providers, spec_index.remove_provider)
]:
for spec in specs.keys():
if self.__whitelist and spec not in self.__whitelist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo(
"Spec '{0}' is not in the whitelist, skipping.".format(
spec))
if self.__blacklist and spec in self.__blacklist:
removed_interfaces.append(spec)
remove_func(spec)
rospy.loginfo(
"Spec '{0}' is in the blacklist, skipping.".format(
spec))
# Remove providers which no longer have an interface
for interface in removed_interfaces:
for provider in spec_index.providers.values():
if provider.implements == interface:
spec_index.remove_provider(provider.name)
self.__spec_index = spec_index
def __populate_default_providers(self):
# Collect available interfaces
interfaces = self.__spec_index.interface_names + self.__spec_index.semantic_interface_names
for interface in interfaces:
# Collect the providers for each interface
providers = [
n for n, p in self.__spec_index.providers.items()
if p.implements == interface
]
if not providers:
# If an interface has not providers, ignore it
rospy.logwarn(
"No providers for capability interface '{0}', not checking for default provider."
.format(interface))
continue
try:
# Try to get the default provider from the corresponding ros parameter
self.__default_providers[interface] = rospy.get_param(
'~defaults/' + interface)
except KeyError:
# No ros parameter set for this capability interface
rospy.logwarn(
"No default provider given for capability interface '{0}'. "
.format(interface))
if len(providers) == 1:
# If there is only one provider, allow it to be the default
rospy.logwarn(
"'{0}' has only one provider, '{1}', using that as the default."
.format(interface, providers[0]))
self.__default_providers[interface] = providers[0]
else:
# Otherwise we can't decide
if self.__missing_default_provider_is_an_error:
rospy.logerr(
"Could not determine a default provider for capability interface '{0}', aborting."
.format(interface))
sys.exit(-1)
else:
rospy.logwarn(
"Could not determine a default provider for capability interface '{0}'."
.format(interface))
continue
# Make sure the given default provider exists
if self.__default_providers[
interface] not in self.__spec_index.provider_names:
rospy.logerr(
"Given default provider '{0}' for interface '{1}' does not exist."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Make sure the given provider implements this interface
if self.__default_providers[interface] not in providers:
rospy.logerr(
"Given default provider '{0}' does not implment interface '{1}'."
.format(self.__default_providers[interface], interface))
sys.exit(-1)
# Update the interface object with the default provider
iface = self.__spec_index.interfaces.get(
interface,
self.__spec_index.semantic_interfaces.get(interface, None))
iface.default_provider = self.__default_providers[interface]
# Summarize defaults
if self.__default_providers:
rospy.loginfo(
"For each available interface, the default provider:")
for interface, provider in self.__default_providers.items():
rospy.loginfo("'{0}'".format(interface))
rospy.loginfo(" => '{0}'".format(provider))
rospy.loginfo("")
else: # pragma: no cover
rospy.logwarn("No runnable Capabilities loaded.")
def __catch_and_log(self, func, *args, **kwargs):
try:
return func(*args, **kwargs)
except Exception as exc:
rospy.logdebug(traceback.format_exc())
rospy.logerr('{0}: {1}'.format(exc.__class__.__name__, str(exc)))
raise
def handle_capability_events(self, event):
"""Callback for handling messages (events) from the /events topic
This callback only process events generated by this node.
:param event: ROS message encapsulating an event
:type event: :py:class:`capabilities.msgs.CapabilityEvent`
"""
return self.__catch_and_log(self._handle_capability_events, event)
def _handle_capability_events(self, event):
# Ignore any publications which we did not send (external publishers)
if event._connection_header['callerid'] != rospy.get_name():
return # pragma: no cover
# Ignore the `server_ready` event
if event.type == event.SERVER_READY:
return
# Update the capability
capability = event.capability
with self.__graph_lock:
if capability not in self.__capability_instances.keys():
rospy.logerr(
"Unknown capability instance: '{0}'".format(capability))
return
instance = self.__capability_instances[capability]
if event.type == event.LAUNCHED:
if instance.canceled: # pragma: no cover
# This is defensive programming, it should not happen
self.__stop_capability(instance.name)
else:
instance.launched(event.pid)
elif event.type == event.TERMINATED:
instance.terminated()
rospy.loginfo("Capability Provider '{0}' for Capability '{1}' "
.format(event.provider, event.capability) +
"has terminated.")
# Update the graph
self.__update_graph()
def __remove_terminated_capabilities(self):
# collect all of the terminated capabilities
terminated = [
x for x in self.__capability_instances.values()
if x.state == 'terminated'
]
# Remove terminated instances
for instance in terminated:
del self.__capability_instances[instance.interface]
# Shutdown unused capabilities
self.__cleanup_graph()
def __cleanup_graph(self):
"""Iterate over the running capabilities and shutdown ones which are no longer needed
For each running capability, if it was not started by the user then look at who depends on it.
If no other capabilities depend on it, then shut it down.
"""
# Collect all running capabilities
running_capabilities = [
x for x in self.__capability_instances.values()
if x.state == 'running'
]
for cap in running_capabilities:
if cap.started_by == USER_SERVICE_REASON:
# Started by user, do not garbage collect this
continue
rdepends = get_reverse_depends(
cap.interface, self.__capability_instances.values())
if rdepends:
# Someone depends on me, do not garbage collect this
rospy.logdebug(
"Keeping the '{0}' provider of the '{1}' interface, ".
format(cap.name, cap.interface) +
"because other running capabilities depend on it.")
continue
if cap.state == 'running':
rospy.loginfo(
"Stopping the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
self.__stop_capability(cap.interface)
elif cap.state == 'waiting': # pragma: no cover
rospy.loginfo(
"Canceling the '{0}' provider of the '{1}' interface, because it has no dependents left."
.format(cap.name, cap.interface))
cap.cancel()
# Else the state is launching, stopping, or terminated
# In which case launching will be caught on the next cleanup
# and the latter two will get cleared out also.
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [
x for x in self.__capability_instances.values()
if x.state == 'waiting'
]
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency_name in instance.depends_on:
if dependency_name not in self.__capability_instances: # pragma: no cover
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on ".
format(instance.name) +
"'{0}', ".format(dependency_name) +
"which is not in the list of capability instances.")
return
dependency = self.__capability_instances[dependency_name]
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path)
# Remove any terminated capabilities
self.__remove_terminated_capabilities()
def __stop_capability(self, name):
if name not in self.__capability_instances:
rospy.logerr("Inconsistent capability run graph, asked to stop " +
"capability '{0}', ".format(name) +
"which is not in the list of capability instances.")
return
capability = self.__capability_instances[name]
rdepends = get_reverse_depends(name,
self.__capability_instances.values())
for cap in rdepends:
self.__stop_capability(cap.interface)
capability.stopped()
self.__launch_manager.stop_capability_provider(capability.pid)
def __get_provider_dependencies(self, provider):
result = []
for interface, dep in provider.dependencies.items():
provider_name = dep.provider or self.__default_providers[interface]
if provider_name not in self.__spec_index.providers:
# This is the case where a provider depends on another interface,
# but the preferred provider does not exist
raise RuntimeError(
"Capability Provider '{0}' not found".format(
provider_name))
dep_provider = self.__spec_index.providers[provider_name]
result.append((dep_provider, provider.name))
return result
def __get_capability_instances_from_provider(self, provider):
instances = []
providers = [(provider, USER_SERVICE_REASON)]
while providers:
curr, reason = providers.pop()
providers.extend(self.__get_provider_dependencies(curr))
curr_path = self.__spec_index.provider_paths[curr.name]
instances.append(
CapabilityInstance(curr, curr_path, started_by=reason))
return instances
def __start_capability(self, capability, preferred_provider):
if capability not in self.__spec_index.interfaces.keys(
) + self.__spec_index.semantic_interfaces.keys():
raise RuntimeError(
"Capability '{0}' not found.".format(capability))
# If no preferred provider is given, use the default
preferred_provider = preferred_provider or self.__default_providers[
capability]
providers = dict([(n, p)
for n, p in self.__spec_index.providers.items()
if p.implements == capability])
if preferred_provider not in providers:
raise RuntimeError(
"Capability Provider '{0}' not found for Capability '{1}'".
format(preferred_provider, capability))
provider = providers[preferred_provider]
instances = self.__get_capability_instances_from_provider(provider)
with self.__graph_lock:
for x in instances:
if x.interface not in self.__capability_instances:
self.__capability_instances[x.interface] = x
self.__update_graph()
return True
def handle_get_capability_specs(self, req):
return self.__catch_and_log(self._handle_get_capability_specs, req)
def _handle_get_capability_specs(self, req):
rospy.loginfo("Servicing request for capability specs...")
response = GetCapabilitySpecsResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in [
'capability_interface', 'semantic_capability_interface',
'capability_provider'
]:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(
raw, path)
if spec_type in [
'capability_interface',
'semantic_capability_interface'
]:
iface.name = '{package}/{name}'.format(
package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[
iface.name]
cs = CapabilitySpec(package_name, spec_type, raw,
default_provider)
response.capability_specs.append(cs)
return response
def handle_get_capability_spec(self, req):
return self.__catch_and_log(self._handle_get_capability_spec, req)
def _handle_get_capability_spec(self, req):
rospy.loginfo(
"Servicing request for get capability spec '{0}'...".format(
req.capability_spec))
response = GetCapabilitySpecResponse()
for package_name, package_dict in self.spec_file_index.items():
for spec_type in [
'capability_interface', 'semantic_capability_interface',
'capability_provider'
]:
for path in package_dict[spec_type]:
with open(path, 'r') as f:
raw = f.read()
default_provider = ''
# If a capability interface, try to lookup the default provider
iface = None
if spec_type == 'capability_interface':
iface = capability_interface_from_string(raw, path)
if spec_type == 'semantic_capability_interface':
iface = semantic_capability_interface_from_string(
raw, path)
if spec_type in [
'capability_interface',
'semantic_capability_interface'
]:
iface.name = '{package}/{name}'.format(
package=package_name, name=iface.name)
if iface.name not in self.__default_providers:
default_provider = ''
else:
default_provider = self.__default_providers[
iface.name]
if iface and iface.name == req.capability_spec:
response.capability_spec = CapabilitySpec(
package_name, spec_type, raw, default_provider)
return response
raise RuntimeError("Could not find requested spec '{0}'".format(
req.capability_spec))
def handle_start_capability(self, req):
return self.__catch_and_log(self._handle_start_capability, req)
def _handle_start_capability(self, req):
msg = "Request to start capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
ret = self.__start_capability(req.capability, req.preferred_provider)
return StartCapabilityResponse(ret or False)
def handle_stop_capability(self, req):
return self.__catch_and_log(self._handle_stop_capability, req)
def _handle_stop_capability(self, req):
rospy.loginfo("Request to stop capability '{0}'".format(
req.capability))
capability = req.capability
if capability not in self.__capability_instances:
raise RuntimeError(
"No Capability '{0}' running".format(capability))
self.__stop_capability(capability)
return StopCapabilityResponse(True)
def handle_reload_request(self, req):
return self.__catch_and_log(self._handle_reload_request, req)
def _handle_reload_request(self, req):
rospy.loginfo("Reloading capabilities...")
self.__load_capabilities()
return EmptyResponse()
def handle_get_interfaces(self, req):
return self.__catch_and_log(self._handle_get_interfaces, req)
def _handle_get_interfaces(self, req):
return GetInterfacesResponse(self.__spec_index.interface_names)
def handle_get_providers(self, req):
return self.__catch_and_log(self._handle_get_providers, req)
def _handle_get_providers(self, req):
if req.interface:
providers = [
n for n, p in self.__spec_index.providers.items()
if p.implements == req.interface
]
else:
providers = self.__spec_index.provider_names
return GetProvidersResponse(providers)
def handle_get_semantic_interfaces(self, req):
return self.__catch_and_log(self._handle_get_semantic_interfaces, req)
def _handle_get_semantic_interfaces(self, req):
if req.interface:
sifaces = [
si.name
for si in self.__spec_index.semantic_interfaces.values()
if si.redefines == req.interface
]
else:
sifaces = self.__spec_index.semantic_interface_names
return GetSemanticInterfacesResponse(sifaces)
def handle_get_running_capabilities(self, req):
return self.__catch_and_log(self._handle_get_running_capabilities, req)
def _handle_get_running_capabilities(self, req):
resp = GetRunningCapabilitiesResponse()
for instance in self.__capability_instances.values():
if instance.state not in ['running']: # pragma: no cover
continue
running_capability = RunningCapability()
running_capability.capability = Capability(instance.interface,
instance.name)
running_capability.started_by = instance.started_by
running_capability.pid = instance.pid
rdepends = get_reverse_depends(
instance.interface, self.__capability_instances.values())
for dep in rdepends:
running_capability.dependent_capabilities.append(
Capability(dep.interface, dep.name))
resp.running_capabilities.append(running_capability)
return resp
def __init__(self, package_paths):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager()
class CapabilityServer(object):
"""A class to expose the :py:class:`discovery.SpecIndex` over a ROS API
"""
def __init__(self, package_paths):
self.__package_paths = package_paths
self.__spec_index = None
self.__graph_lock = threading.Lock()
self.__capability_instances = {}
self.__launch_manager = LaunchManager()
def spin(self):
"""Starts the capability server by setting up ROS comms, then spins"""
self.__load_capabilities()
self.__start_capability_service = rospy.Service(
'start_capability', StartCapability, self.handle_start_capability)
self.__start_capability_service = rospy.Service(
'stop_capability', StopCapability, self.handle_stop_capability)
self.__reload_service = rospy.Service(
'reload_capabilities', Empty, self.handle_reload_request)
self.__interfaces_service = rospy.Service(
'get_interfaces', GetInterfaces, self.handle_get_interfaces)
self.__providers_service = rospy.Service(
'get_providers', GetProviders, self.handle_get_providers)
self.__semantic_interfaces_service = rospy.Service(
'get_semantic_interfaces', GetSemanticInterfaces,
self.handle_get_semantic_interfaces)
rospy.Subscriber(
'events', CapabilityEvent, self.handle_capability_events)
rospy.loginfo("Capability Server Ready")
rospy.spin()
def shutdown(self):
"""Stops the capability server and cleans up any running processes"""
self.__launch_manager.stop()
def handle_capability_events(self, event):
"""Callback for handling messages (events) from the /events topic
This callback only process events generated by this node.
:param event: ROS message encapsulating an event
:type event: :py:class:`capabilities.msgs.CapabilityEvent`
"""
# Ignore any publications which we did not send (external publishers)
if event._connection_header['callerid'] != rospy.get_name():
return
# Update the capability
capability = event.capability
with self.__graph_lock:
if capability not in self.__capability_instances:
rospy.logerr("Unknown capability instance: '{0}'"
.format(capability))
return
instance = self.__capability_instances[capability]
if event.type == event.LAUNCHED:
if instance.canceled:
self.__stop_capability(instance.name)
else:
instance.launched(event.pid)
elif event.type == event.TERMINATED:
instance.terminated()
rospy.loginfo(
"Capability Provider '{0}' for Capability '{1}' "
.format(event.provider, event.capability) +
"has terminated.")
# Stop or cancel any dependents
reverse_depends = get_reverse_depends(
instance, self.__capability_instances)
for dependency in reverse_depends:
if dependency.state == 'running':
self.__stop_capability(dependency.name)
if dependency.state == 'waiting':
dependency.cancel()
elif event.type == event.STOPPED:
self.__capability_instances[capability].stopped()
# Update the graph
self.__update_graph()
def __update_graph(self):
# collect all of the waiting capabilities
waiting = [x
for x in self.__capability_instances.values()
if x.state == 'waiting']
# If any of the waiting have no blocking dependencies start them
for instance in waiting:
blocking_dependencies = []
for dependency in instance.depends_on:
if dependency not in [x.name for x in self.__capability_instances]:
rospy.logerr(
"Inconsistent capability run graph, '{0}' depends on "
.format(instance.name) + "'{1}', ".format(dependency) +
"which is not in the list of capability instances.")
return
if dependency.state != 'running':
blocking_dependencies.append(dependency)
if not blocking_dependencies:
instance.launch()
self.__launch_manager.run_capability_provider(
instance.provider, instance.provider_path
)
# collect all of the terminated capabilities
terminated = [x
for x in self.__capability_instances.values()
if x.state == 'terminated']
# Remove terminated instances
for instance in terminated:
del self.__capability_instances[instance.interface]
def __stop_capability(self, name):
if name not in self.__capability_instances:
rospy.logerr("Inconsistent capability run graph, asked to stop " +
"capability '{0}', ".format(name) +
"which is not in the list of capability instances.")
return
self.__launch_manager.stop_capability_provider(
self.__capability_instances[name].pid)
def __get_capability_instances_from_provider(self, provider):
def get_provider_dependencies(provider):
result = []
for interface, dep in provider.dependencies:
provider_name = dep.provider
if provider_name is None:
capability = dep.capability
providers = self.__get_providers_for_interface(capability)
provider_name = providers.keys()[0]
if provider_name not in self.__spec_index.providers:
raise RuntimeError("Capability Provider '{0}' not found"
.format(provider_name))
dep_provider = self.__spec_index.providers[provider_name]
result.append(dep_provider)
return result
instances = []
providers = [provider]
while providers:
curr = providers.pop()
providers.extend(get_provider_dependencies(curr))
curr_path = self.__spec_index.provider_paths[curr.name]
instances.append(CapabilityInstance(curr, curr_path))
return instances
def __get_providers_for_interface(self, interface):
providers = dict([(p.name, p)
for p in self.__spec_index.providers.values()
if p.implements == interface])
if not providers:
raise RuntimeError("No providers for Capability '{0}'"
.format(interface))
return providers
def __start_capability(self, capability, preferred_provider):
if capability not in self.__spec_index.interfaces:
raise RuntimeError("Capability '{0}' not found.".format(capability))
providers = self.__get_providers_for_interface(capability)
if preferred_provider:
if preferred_provider not in providers:
raise RuntimeError(
"Capability Provider '{0}' not found for Capability '{1}'"
.format(preferred_provider, capability))
provider = providers[preferred_provider]
else:
provider = providers.values()[0]
instances = self.__get_capability_instances_from_provider(provider)
with self.__graph_lock:
self.__capability_instances.update(dict([(x.interface, x) for x in instances]))
self.__update_graph()
return True
def __load_capabilities(self):
package_index = package_index_from_package_path(self.__package_paths)
spec_file_index = spec_file_index_from_package_index(package_index)
spec_index, errors = spec_index_from_spec_file_index(spec_file_index)
if errors:
rospy.logerr("Errors were encountered loading capabilities:")
for error in errors:
if type(error) == DuplicateNameException:
rospy.logerr(" DuplicateNameException: " + str(error))
elif type(error) == InterfaceNameNotFoundException:
rospy.logerr(" InterfaceNameNotFoundException: " +
str(error))
else:
rospy.logerr(" " + str(type(error)) + ": " + str(error))
self.__spec_index = spec_index
def handle_start_capability(self, req):
msg = "Request to start capability '{0}'".format(req.capability)
if req.preferred_provider:
msg += " with provider '{0}'".format(req.preferred_provider)
rospy.loginfo(msg)
ret = self.__start_capability(req.capability, req.preferred_provider)
return StartCapabilityResponse(ret or False)
def handle_stop_capability(self, req):
rospy.loginfo("Request to stop capability '{0}'".format(req.capability))
capability = req.capability
if capability not in self.__capability_instances:
raise RuntimeError("No Capability '{0}' running".format(capability))
self.__capability_instances[capability].stopped()
self.__stop_capability(req.capability)
return StopCapabilityResponse(True)
def handle_reload_request(self, req):
rospy.loginfo("Reloading capabilities...")
self.__load_capabilities()
return EmptyResponse()
def handle_get_interfaces(self, req):
return GetInterfacesResponse(self.__spec_index.interface_names)
def handle_get_providers(self, req):
if req.interface:
providers = [p.name
for p in self.__spec_index.providers.values()
if p.implements == req.interface]
else:
providers = self.__spec_index.provider_names
return GetProvidersResponse(providers)
def handle_get_semantic_interfaces(self, req):
if req.interface:
sifaces = [si.name
for si in self.__spec_index.semantic_interfaces.values()
if si.redefines == req.interface]
else:
sifaces = self.__spec_index.semantic_interface_names
return GetSemanticInterfacesResponse(sifaces)
