def establish_bond(self, timeout=None):
"""Establishes a bond using the ~establish_bond service call
The bond id which is returned by the service call is stored internally
and used implicitly by the use/free capabilities functions.
If establish_bond had previously been called, then the old bond will be
broken, which will result in any capability uses under that bond to be
implicitly freed.
This function is called implicitly by :py:method:`use_capability` if
no bond exists.
This function will block waiting for the service call to become
available and to wait for the bond to be established. In both cases
the timeout parameter is used.
None is returned if the timeout occurs while waiting on the service
to become available or while waiting for the bond to form.
:param timeout: time in seconds to wait for the service to be available
:returns: the bond_id received from the server or None on failure
:rtype: str
"""
if self.__wait_for_service(self.__establish_bond, timeout) is False:
return None
resp = self.__establish_bond()
if not resp.bond_id:
return None
self._bond_id = resp.bond_id
self._bond = Bond('{0}/bonds'.format(self._name), self._bond_id)
self._bond.start()
if self._bond.wait_until_formed(rospy.Duration(timeout)) is False:
return None
return self._bond_id
def establish_bond(self, timeout=None):
"""Establishes a bond using the ``~establish_bond`` service call
The bond id which is returned by the service call is stored internally
and used implicitly by the use/free capabilities functions.
If :py:meth:`establish_bond` had previously been called, then the old bond will be
broken, which will result in any capability uses under that bond to be
implicitly freed.
This function is called implicitly by :py:meth:`use_capability` if
no bond exists.
This function will block waiting for the service call to become
available and to wait for the bond to be established. In both cases
the timeout parameter is used.
None is returned if the timeout occurs while waiting on the service
to become available or while waiting for the bond to form.
:param timeout: time in seconds to wait for the service to be available
:type timeout: float
:returns: the bond_id received from the server or :py:obj:`None` on failure
:rtype: :py:obj:`str`
"""
if self.__wait_for_service(self.__establish_bond, timeout) is False:
return None
resp = self.__establish_bond()
if not resp.bond_id: # pragma: no cover
return None
self._bond_id = resp.bond_id
self._bond = Bond('{0}/bonds'.format(self._name), self._bond_id)
self._bond.start()
timeout_dur = None if timeout is None else rospy.Duration.from_sec(
timeout)
if self._bond.wait_until_formed(
timeout_dur) is False: # pragma: no cover
return None
return self._bond_id
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)
class CapabilitiesClient(object):
"""Single point of entry for interacting with a remote capability server.
Instantiation of this class does not check to see if the underlying
services are available, call :py:meth:`wait_for_services` if you want
to ensure that the services are available before continuing.
:param capability_server_node_name: name of the remote capability server
:type capability_server_node_name: str
"""
def __init__(self, capability_server_node_name='/capability_server'):
self._name = capability_server_node_name
self._bond = None
self._bond_id = None
self._used_capabilities = set([])
self._services = {}
# Create service proxy for establish_bond
service_name = '{0}/establish_bond'.format(capability_server_node_name)
self.__establish_bond = rospy.ServiceProxy(service_name, EstablishBond)
self._services['establish_bond'] = self.__establish_bond
# Create service proxy for free_capability
service_name = '{0}/free_capability'.format(capability_server_node_name)
self.__free_capability = rospy.ServiceProxy(service_name, FreeCapability)
self._services['free_capability'] = self.__free_capability
# Create service proxy for use_capability
service_name = '{0}/use_capability'.format(capability_server_node_name)
self.__use_capability = rospy.ServiceProxy(service_name, UseCapability)
self._services['use_capability'] = self.__use_capability
# Register atexit shutdown
atexit.register(self.shutdown)
def wait_for_services(self, timeout=None, services=None):
"""Blocks until the requested services are available.
Services are waited on one at a time. Therefore, if a non-None value
for timeout is given, and one or more services are being waited on,
the full timeout will be given to each wait, meaning that the total
run time of this function can exceed the timeout provided.
:param timeout: Seconds to wait for the services before returning False.
If None is passed, then this will block indefinitely until the
services are available.
:type timeout: float
:param services: List of services to wait on.
If None is passed, then this will check all the services.
:type services: list
:returns: :py:obj:`True` is the services are available, :py:obj:`False` otherwise (timeout)
:rtype: :py:obj:`bool`
"""
services = self._services.keys() if services is None else services
assert isinstance(services, list), services
for service in services:
if service not in self._services:
raise ValueError(
"Service '{0}' is not a valid service and cannot be waited on. These are the valid services: {1}"
.format(service, list(self._services.keys())))
if self.__wait_for_service(self._services[service], timeout) is False:
return False
return True
def __wait_for_service(self, service, timeout):
try:
service.wait_for_service(timeout)
except rospy.ROSException:
rospy.logwarn("Timed out after waiting '{0}' seconds for service '{1}' to be available."
.format(timeout, service.resolved_name))
return False
return True
def establish_bond(self, timeout=None):
"""Establishes a bond using the ``~establish_bond`` service call
The bond id which is returned by the service call is stored internally
and used implicitly by the use/free capabilities functions.
If :py:meth:`establish_bond` had previously been called, then the old bond will be
broken, which will result in any capability uses under that bond to be
implicitly freed.
This function is called implicitly by :py:meth:`use_capability` if
no bond exists.
This function will block waiting for the service call to become
available and to wait for the bond to be established. In both cases
the timeout parameter is used.
None is returned if the timeout occurs while waiting on the service
to become available or while waiting for the bond to form.
:param timeout: time in seconds to wait for the service to be available
:type timeout: float
:returns: the bond_id received from the server or :py:obj:`None` on failure
:rtype: :py:obj:`str`
"""
if self.__wait_for_service(self.__establish_bond, timeout) is False:
return None
resp = self.__establish_bond()
if not resp.bond_id: # pragma: no cover
return None
self._bond_id = resp.bond_id
self._bond = Bond('{0}/bonds'.format(self._name), self._bond_id)
self._bond.start()
timeout_dur = None if timeout is None else rospy.Duration.from_sec(timeout)
if self._bond.wait_until_formed(timeout_dur) is False: # pragma: no cover
return None
return self._bond_id
def free_capability(self, capability_interface, timeout=None):
"""Free's a previously used capability.
Calls the ``~free_capability`` service, which effectively decrements
the internal reference count for that capability in the remote
capability server.
If that results in a reference count of zero,
then the capability server will shutdown that capability automatically.
:param capability_interface: Name of the capability interface to free up
:type capability_interface: str
:param timeout: time to wait on service to be available (optional)
:type timeout: float
:returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
:rtype: :py:obj:`bool`
:raises: CapabilityNotInUseException if the capability has not been previously used
:raises: CapabilityNotRunningException if the capability has already been stopped
which can be caused by a capability it depends on stopping
:raises: ServiceNotAvailableException if the required service is not available
after the timeout has expired
"""
if capability_interface not in self._used_capabilities:
rospy.logerr("Cannot free capability interface '{0}', because it was not first used."
.format(capability_interface))
CapabilityNotInUseException(capability_interface)
if self.__wait_for_service(self.__free_capability, timeout) is False:
raise ServiceNotAvailableException(self.__free_capability.resolved_name)
try:
self.__free_capability.call(capability_interface, self._bond_id)
except rospy.ServiceException as exc:
exc_str = "{0}".format(exc)
if 'Cannot free Capability' in exc_str and 'because it is not running' in exc_str:
raise CapabilityNotRunningException(capability_interface)
return True
def shutdown(self):
"""Cleanly frees any used capabilities."""
if self._bond:
self._bond.break_bond()
def use_capability(self, capability_interface, preferred_provider=None, timeout=None):
"""Declares that this capability is being used.
Calls the ``~use_capability`` service, and opens a bond with capability server if none exists.
This will cause the capability to be started, if it has not been already, and the
internal reference count for that capability in the capability server is incremented.
If the bond fails (i.e. this program crashes) then the reference is decremented automatically.
The reference is also decremented if :py:meth:`free_capability` is called.
:param capability_interface: Name of the capability interface to use
:type capability_interface: str
:param preferred_provider: preferred provider or None for default provider (optional)
:type preferred_provider: str
:param timeout: time to wait on service to be available (optional)
:type timeout: float
:returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
:rtype: :py:obj:`bool`
:raises: ServiceNotAvailableException if the required service is not available
after the timeout has expired
:raise: CannotEstablishBondException is the bond with the capability_server cannot be established
"""
# If no bond has been established, establish one first
if self._bond is None:
if self.establish_bond(timeout) is None: # pragma: no cover
raise CannotEstablishBondException()
if self.__wait_for_service(self.__use_capability, timeout) is False: # pragma: no cover
raise ServiceNotAvailableException(self.__use_capability.resolved_name)
self.__use_capability.call(capability_interface, preferred_provider or '', self._bond_id)
self._used_capabilities.add(capability_interface)
return True
class CapabilitiesClient(object):
"""Single point of entry for interacting with a remote capability server.
Instantiation of this class does not check to see if the underlying
services are available, call :py:meth:`wait_for_services` if you want
to ensure that the services are available before continuing.
:param capability_server_node_name: name of the remote capability server
:type capability_server_node_name: str
"""
def __init__(self, capability_server_node_name='/capability_server'):
self._name = capability_server_node_name
self._bond = None
self._bond_id = None
self._used_capabilities = set([])
self._services = {}
# Create service proxy for establish_bond
service_name = '{0}/establish_bond'.format(capability_server_node_name)
self.__establish_bond = rospy.ServiceProxy(service_name, EstablishBond)
self._services['establish_bond'] = self.__establish_bond
# Create service proxy for free_capability
service_name = '{0}/free_capability'.format(
capability_server_node_name)
self.__free_capability = rospy.ServiceProxy(service_name,
FreeCapability)
self._services['free_capability'] = self.__free_capability
# Create service proxy for use_capability
service_name = '{0}/use_capability'.format(capability_server_node_name)
self.__use_capability = rospy.ServiceProxy(service_name, UseCapability)
self._services['use_capability'] = self.__use_capability
# Register atexit shutdown
atexit.register(self.shutdown)
def wait_for_services(self, timeout=None, services=None):
"""Blocks until the requested services are available.
Services are waited on one at a time. Therefore, if a non-None value
for timeout is given, and one or more services are being waited on,
the full timeout will be given to each wait, meaning that the total
run time of this function can exceed the timeout provided.
:param timeout: Seconds to wait for the services before returning False.
If None is passed, then this will block indefinitely until the
services are available.
:type timeout: float
:param services: List of services to wait on.
If None is passed, then this will check all the services.
:type services: list
:returns: :py:obj:`True` is the services are available, :py:obj:`False` otherwise (timeout)
:rtype: :py:obj:`bool`
"""
services = self._services.keys() if services is None else services
assert isinstance(services, list), services
for service in services:
if service not in self._services:
raise ValueError(
"Service '{0}' is not a valid service and cannot be waited on. These are the valid services: {1}"
.format(service, list(self._services.keys())))
if self.__wait_for_service(self._services[service],
timeout) is False:
return False
return True
def __wait_for_service(self, service, timeout):
try:
service.wait_for_service(timeout)
except rospy.ROSException:
rospy.logwarn(
"Timed out after waiting '{0}' seconds for service '{1}' to be available."
.format(timeout, service.resolved_name))
return False
return True
def establish_bond(self, timeout=None):
"""Establishes a bond using the ``~establish_bond`` service call
The bond id which is returned by the service call is stored internally
and used implicitly by the use/free capabilities functions.
If :py:meth:`establish_bond` had previously been called, then the old bond will be
broken, which will result in any capability uses under that bond to be
implicitly freed.
This function is called implicitly by :py:meth:`use_capability` if
no bond exists.
This function will block waiting for the service call to become
available and to wait for the bond to be established. In both cases
the timeout parameter is used.
None is returned if the timeout occurs while waiting on the service
to become available or while waiting for the bond to form.
:param timeout: time in seconds to wait for the service to be available
:type timeout: float
:returns: the bond_id received from the server or :py:obj:`None` on failure
:rtype: :py:obj:`str`
"""
if self.__wait_for_service(self.__establish_bond, timeout) is False:
return None
resp = self.__establish_bond()
if not resp.bond_id: # pragma: no cover
return None
self._bond_id = resp.bond_id
self._bond = Bond('{0}/bonds'.format(self._name), self._bond_id)
self._bond.start()
timeout_dur = None if timeout is None else rospy.Duration.from_sec(
timeout)
if self._bond.wait_until_formed(
timeout_dur) is False: # pragma: no cover
return None
return self._bond_id
def free_capability(self, capability_interface, timeout=None):
"""Free's a previously used capability.
Calls the ``~free_capability`` service, which effectively decrements
the internal reference count for that capability in the remote
capability server.
If that results in a reference count of zero,
then the capability server will shutdown that capability automatically.
:param capability_interface: Name of the capability interface to free up
:type capability_interface: str
:param timeout: time to wait on service to be available (optional)
:type timeout: float
:returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
:rtype: :py:obj:`bool`
"""
if capability_interface not in self._used_capabilities:
rospy.logerr(
"Cannot free capability interface '{0}', because it was not first used."
.format(capability_interface))
return False
if self.__wait_for_service(self.__free_capability, timeout) is False:
return False
self.__free_capability.call(capability_interface, self._bond_id)
return True
def shutdown(self):
"""Cleanly frees any used capabilities."""
if self._bond:
self._bond.break_bond()
def use_capability(self,
capability_interface,
preferred_provider=None,
timeout=None):
"""Declares that this capability is being used.
Calls the ``~use_capability`` service, and opens a bond with capability server if none exists.
This will cause the capability to be started, if it has not been already, and the
internal reference count for that capability in the capability server is incremented.
If the bond fails (i.e. this program crashes) then the reference is decremented automatically.
The reference is also decremented if :py:meth:`free_capability` is called.
:param capability_interface: Name of the capability interface to use
:type capability_interface: str
:param preferred_provider: preferred provider or None for default provider (optional)
:type preferred_provider: str
:param timeout: time to wait on service to be available (optional)
:type timeout: float
:returns: :py:obj:`True` if successful, otherwise :py:obj:`False`
:rtype: :py:obj:`bool`
"""
# If no bond has been established, establish one first
if self._bond is None:
if self.establish_bond(timeout) is None: # pragma: no cover
return False
if self.__wait_for_service(self.__use_capability,
timeout) is False: # pragma: no cover
return False
self.__use_capability.call(capability_interface, preferred_provider
or '', self._bond_id)
self._used_capabilities.add(capability_interface)
return True
