Exemplo n.º 1
0
 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)
Exemplo n.º 2
0
 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)
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
0
 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()
Exemplo n.º 8
0
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)