Source code for capabilities.server

# Software License Agreement (BSD License)
# Copyright (c) 2013, Open Source Robotics Foundation, Inc.
# All rights reserved.
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Open Source Robotics Foundation, Inc. nor
#    the names of its contributors may be used to endorse or promote
#    products derived from this software without specific prior
#    written permission.

# Author: Tully Foote <>
# Author: William Woodall <>

This module implements the Capability server.

The Capability server provides access to queries and services related
to capabilities.

from __future__ import print_function

import argparse
import logging
import os
import sys
import threading
import traceback
import uuid

import rospy

from bondpy.bondpy import Bond

from std_srvs.srv import Empty
from std_srvs.srv import EmptyResponse

from capabilities.srv import EstablishBond
from capabilities.srv import EstablishBondResponse
from capabilities.srv import GetCapabilitySpec
from capabilities.srv import GetCapabilitySpecResponse
from capabilities.srv import FreeCapability
from capabilities.srv import FreeCapabilityResponse
from capabilities.srv import GetCapabilitySpecs
from capabilities.srv import GetCapabilitySpecsResponse
from capabilities.srv import GetInterfaces
from capabilities.srv import GetInterfacesResponse
from capabilities.srv import GetNodeletManagerName
from capabilities.srv import GetNodeletManagerNameResponse
from capabilities.srv import GetProviders
from capabilities.srv import GetProvidersResponse
from capabilities.srv import GetSemanticInterfaces
from capabilities.srv import GetSemanticInterfacesResponse
from capabilities.srv import GetRemappings
from capabilities.srv import GetRemappingsResponse
from capabilities.srv import GetRunningCapabilities
from capabilities.srv import GetRunningCapabilitiesResponse
from capabilities.srv import StartCapability
from capabilities.srv import StartCapabilityResponse
from capabilities.srv import StopCapability
from capabilities.srv import StopCapabilityResponse
from capabilities.srv import UseCapability
from capabilities.srv import UseCapabilityResponse

from capabilities.discovery import package_index_from_package_path
from capabilities.discovery import spec_file_index_from_package_index
from capabilities.discovery import spec_index_from_spec_file_index

from capabilities.launch_manager import _special_nodelet_manager_capability
from capabilities.launch_manager import LaunchManager

from capabilities.msg import Capability
from capabilities.msg import CapabilityEvent
from capabilities.msg import CapabilitySpec
from capabilities.msg import Remapping
from capabilities.msg import RunningCapability

from capabilities.specs.interface import capability_interface_from_string
from capabilities.specs.semantic_interface import semantic_capability_interface_from_string

USER_SERVICE_REASON = 'user service call'

## Hack to squelch output from Service call failure ##

from rospy.impl import tcpros_service

def custom__handle_request(self, transport, request):  # pragma: no cover
    import struct
    from rospy.impl.tcpros_service import convert_return_to_response
    from rospy.service import ServiceException
        # convert return type to response Message instance
        response = convert_return_to_response(self.handler(request), self.response_class)
        self.seq += 1
        # ok byte
        transport.write_buff.write(struct.pack('<B', 1))
        transport.send_message(response, self.seq)
    except ServiceException as e:
        rospy.core.rospydebug("handler raised ServiceException: %s" % e)
        self._write_service_error(transport, "service cannot process request: %s" % e)
    except Exception as e:
        # rospy.logerr("Error processing request: %s\n%s" % (e, traceback.print_exc()))
        self._write_service_error(transport, "error processing request: %s" % e)

tcpros_service.ServiceImpl._handle_request = custom__handle_request

## End hacks ##

[docs]class CapabilityInstance(object): """Encapsulates the state of an instance of a Capability Provider This class encapsulates the state of the capability instance and provides methods for changing the states of the instance. """ def __init__(self, provider, provider_path, started_by='unknown'): self.__state = 'waiting' = self.provider = provider self.provider_path = provider_path self.interface = provider.implements = None self.depends_on = [x for x in provider.dependencies] self.canceled = False self.started_by = started_by self.bonds = {} # {bond_id: reference_count} @property def reference_count(self): return sum(list(self.bonds.values())) @property def state(self): """Get the current state of the CapabilityInstance""" return self.__state
[docs] def launch(self): """Change to the 'launching' state Fails to transition if the current state is not 'waiting'. :returns: True if transition is successful, False otherwise :rtype: bool """ if self.state != 'waiting': rospy.logerr( "Capability Provider '{0}' ".format( + "cannot transition to 'launching' from anything but " + "'waiting', current state is '{0}'".format(self.state)) return False self.__state = 'launching' return True
[docs] def cancel(self): """Cancels the instance, which can only be done while it is still 'waiting' Fails to cancel if the current state is not 'waiting'. "Canceling" is achieved by setting the canceled member variable to True. :returns: True if canceling is successful, False otherwise :rtype: bool """ if self.state != 'waiting': rospy.logerr( "Capability Instance '{0}' ".format( + "cannot be canceled from anything but " + "'waiting', current state is '{0}'".format(self.state)) return False self.canceled = True return True
[docs] def launched(self, pid): """Called once the instance is "launched", changes state to 'running' Fails to transition if the current state is not 'launching'. If successful, the state changes to 'running'. :param pid: process ID of the instance being tracked :type pid: int :returns: True if transition is successful, False otherwise :rtype: bool """ = pid if self.state != 'launching': rospy.logerr( "Capability Instance '{0}' ".format( + "cannot transition to 'running' from anything but " + "'launching', current state is '{0}'".format(self.state)) return False self.__state = 'running' return True
[docs] def stopped(self): """Change to the 'stopping' state Fails to transition if the current state is not either 'running' or 'launching'. :returns: True if transition is successful, False otherwise :rtype: bool """ if self.state not in ['running', 'launching']: rospy.logerr( "Capability Instance '{0}' ".format( + "cannot transition to 'stopping' from anything but " + "'launching' or 'running', " + "current state is '{0}'".format(self.state)) return False self.__state = 'stopping' return True
[docs] def terminated(self): """Called when the instance has terminated, transitions to the 'terminated' state Fails to transition if the current state is not 'stopping'. :returns: True if transition is successful, False otherwise :rtype: bool """ result = True if self.state != 'stopping': rospy.logerr( "Capability Instance '{0}' ".format( + "terminated unexpectedly, it was previously in the " + "'{0}' state.".format(self.state)) result = False self.__state = 'terminated' return result
[docs]def get_reverse_depends(name, capability_instances): """Gets the reverse dependencies of a given Capability :param name: Name of the Capability which the instances might depend on :type name: str :param capability_instances: list of instances to search for having a dependency on the given Capability :type capability_instances: :py:obj:`list` of :py:class:`CapabilityInstance` :returns: A list of :py:class:`CapabilityInstance`'s which depend on the given Capability name :rtype: :py:obj:`list` of :py:class:`CapabilityInstance` """ rdepends = [] for instance in capability_instances: if name in instance.depends_on: rdepends.append(instance) return rdepends
[docs]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 = {}
[docs] 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()
[docs] 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( 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
[docs] 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( else: instance.launched( 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.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.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.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( + "'{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(, name)) self.__stop_capability(cap.interface) capability.stopped() self.__launch_manager.stop_capability_provider( 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, 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[] 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 = 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']: = '{package}/{name}'.format(package=package_name, if not in self.__default_providers: default_provider = '' else: default_provider = self.__default_providers[] 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 = 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']: = '{package}/{name}'.format(package=package_name, if not in self.__default_providers: default_provider = '' else: default_provider = self.__default_providers[] if iface and == 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 != 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( 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 = [ 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, running_capability.started_by = instance.started_by = rdepends = get_reverse_depends(instance.interface, self.__capability_instances.values()) for dep in rdepends: running_capability.dependent_capabilities.append(Capability(dep.interface, 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) 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
def create_parser(): parser = argparse.ArgumentParser(description="Runs the capability server") add = parser.add_argument add('package_path', nargs='?', help="Overrides ROS_PACKAGE_PATH when discovering capabilities") add('--screen', '-s', action='store_true', default=False, help="Passes `--screen` down to roslaunch, `use_screen` rosparam takes priority.") return parser def main(sysargv=None): sys.argv = rospy.myargv(argv=sys.argv) parser = create_parser() args = parser.parse_args(sysargv) ros_package_path = args.package_path or os.getenv('ROS_PACKAGE_PATH', '') ros_package_path = [x for x in ros_package_path.split(':') if x] if not ros_package_path: sys.exit('No package paths specified, set ROS_PACKAGE_PATH or ' 'pass them as an argument') # Extend the ROS_PACKAGE_PATH os.environ['ROS_PACKAGE_PATH'] = ':'.join( os.getenv('ROS_PACKAGE_PATH', '').split(':') + ros_package_path) rospy.init_node('capability_server') capability_server = CapabilityServer(ros_package_path, screen=args.screen) capability_server.spin() capability_server.shutdown()