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))) 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): 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)