diff --git a/rclpy/__init__.py.in b/rclpy/__init__.py.in
index d21848bcb..e64ea1ba7 100644
--- a/rclpy/__init__.py.in
+++ b/rclpy/__init__.py.in
@@ -27,7 +27,9 @@ del source_path
for __execfile in __execfiles:
with open(__execfile, 'r') as __fh:
- exec(__fh.read())
+ __code_object = compile(__fh.read(), __execfile, 'exec')
+ exec(__code_object)
+ del __code_object
del __fh
del __execfile
del __execfiles
diff --git a/rclpy/package.xml b/rclpy/package.xml
index 79357af59..4d3ba29b5 100644
--- a/rclpy/package.xml
+++ b/rclpy/package.xml
@@ -17,9 +17,9 @@
ament_index_python
ament_cmake_nose
- ament_index_python
ament_lint_auto
ament_lint_common
+ launch
std_msgs
rosidl_generator_py
rosidl_typesupport_opensplice_c
diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py
index 55f35540e..ecdcc358d 100644
--- a/rclpy/rclpy/__init__.py
+++ b/rclpy/rclpy/__init__.py
@@ -12,23 +12,38 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+import logging
import os
+import sys
-from rclpy import selector
+from rclpy.exceptions import InvalidRCLPYImplementation
+from rclpy.impl import implementation_singleton
+from rclpy.impl import rmw_implementation_tools
from rclpy.node import Node
-_rclpy = None
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
-def init(args):
- global _rclpy
- rclpy_rmw_env = os.getenv('RCLPY_IMPLEMENTATION')
- if rclpy_rmw_env:
- selector.select(rclpy_rmw_env)
- _rclpy = selector.import_rmw_implementation()
+def init(args=None):
+ rclpy_rmw_env = os.getenv('RCLPY_IMPLEMENTATION', None)
+ if rclpy_rmw_env is not None:
+ available_rmw_implementations = rmw_implementation_tools.get_rmw_implementations()
+ if rclpy_rmw_env not in available_rmw_implementations:
+ logger = logging.getLogger('rclpy')
+ logger.error(
+ "The rmw implementation specified in 'RCLPY_IMPLEMENTATION={0}', "
+ "is not one of the available implementations: {1}"
+ .format(rclpy_rmw_env, available_rmw_implementations)
+ )
+ raise InvalidRCLPYImplementation()
+ rmw_implementation_tools.select_rmw_implementation(rclpy_rmw_env)
+ # This line changes what is in "_rclpy" to be the rmw implementation module that was imported.
+ implementation_singleton.set_rclpy_implementation(
+ rmw_implementation_tools.import_rmw_implementation()
+ )
- assert _rclpy is not None, 'Could not load any RMW implementation'
- return _rclpy.rclpy_init(args)
+ # Now we can use _rclpy to call the implementation specific rclpy_init().
+ return _rclpy.rclpy_init(args if args is not None else sys.argv)
def create_node(node_name):
@@ -36,21 +51,22 @@ def create_node(node_name):
return Node(node_handle)
-def spin(node):
+def spin_once(node):
wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
_rclpy.rclpy_wait_set_init(wait_set, len(node.subscriptions), 0, 0)
- while ok():
- _rclpy.rclpy_wait_set_clear_subscriptions(wait_set)
- for subscription in node.subscriptions:
- _rclpy.rclpy_wait_set_add_subscription(wait_set, subscription.subscription_handle)
- _rclpy.rclpy_wait(wait_set)
+ _rclpy.rclpy_wait_set_clear_subscriptions(wait_set)
+ for subscription in node.subscriptions:
+ _rclpy.rclpy_wait_set_add_subscription(wait_set, subscription.subscription_handle)
- msg = _rclpy.rclpy_take(subscription.subscription_handle, subscription.msg_type)
+ _rclpy.rclpy_wait(wait_set)
- if msg:
- subscription.callback(msg)
+ # TODO(wjwwood): properly implement this by checking the contents of the wait_set.
+ for subscription in node.subscriptions:
+ msg = _rclpy.rclpy_take(subscription.subscription_handle, subscription.msg_type)
+ if msg:
+ subscription.callback(msg)
def ok():
@@ -61,8 +77,5 @@ def shutdown():
return _rclpy.rclpy_shutdown()
-def get_implementation_identifier():
- if _rclpy:
- return _rclpy.rclpy_get_implementation_identifier()
- else:
- return None
+def get_rmw_implementation_identifier():
+ return _rclpy.rclpy_get_rmw_implementation_identifier()
diff --git a/rclpy/rclpy/exceptions.py b/rclpy/rclpy/exceptions.py
new file mode 100644
index 000000000..857c72121
--- /dev/null
+++ b/rclpy/rclpy/exceptions.py
@@ -0,0 +1,34 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+class NotInitializedException(Exception):
+ """Raised when the rclpy implementation is accessed before rclpy.init()."""
+
+ def __init__(self, *args):
+ Exception.__init__(self, 'rclpy.init() has not been called', *args)
+
+
+class ImplementationAlreadyImportedException(Exception):
+ """Raised on select_rmw_implemenation() after import_rmw_implementation() has been called."""
+
+ def __init__(self, *args):
+ Exception.__init__(self, 'rmw implementation already imported', *args)
+
+
+class InvalidRCLPYImplementation(Exception):
+ """Raised when an invalid RCLPYImplementation is requested."""
+
+ def __init__(self, *args):
+ Exception.__init__(self, 'requested invalid rmw implementation', *args)
diff --git a/rclpy/rclpy/impl/__init__.py b/rclpy/rclpy/impl/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py
new file mode 100644
index 000000000..b54cc614c
--- /dev/null
+++ b/rclpy/rclpy/impl/implementation_singleton.py
@@ -0,0 +1,65 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Provide singleton access to the rclpy implementation.
+
+The singleton is called ``rclpy_implementation`` and is in this module.
+For example, you might use it like this:
+
+.. code::
+
+ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+ _rclpy.rclpy_init()
+ while _rclpy.rclpy_ok():
+ # ...
+
+Before you call :py:func:`rclpy.init` this singleton will point to an instance
+of the :py:class:`ImplementationPlaceholder` class, which will raise a
+:py:class:`rclpy.exceptions.NotInitializedException` when ever accessed.
+After initialization it will point to the rclpy implementation C module.
+"""
+
+from rclpy.exceptions import NotInitializedException
+from rclpy.impl.object_proxy import ObjectProxy
+
+
+class ImplementationPlaceholder:
+ """Placeholder for the rclpy implementation module.
+
+ This class will raise a :py:class:`NotInitializedException` when used.
+ """
+
+ def __getattr__(self, key):
+ if key in ['__repr__']:
+ return object.__getattr__(key)
+ raise NotInitializedException()
+
+rclpy_implementation = ObjectProxy(ImplementationPlaceholder())
+
+
+def set_rclpy_implementation(implementation):
+ """Set the rclpy implementation singleton."""
+ # Update the ObjectProxy to point to the rmw implementation specific module.
+ rclpy_implementation.__actual__ = implementation
+
+
+def rclpy_implementation_is_placeholder(implementation=None):
+ """Return True if the implementation is a placeholder, else False.
+
+ :param implementation: implementation to check, defaults to the singleton
+ :returns: bool
+ """
+ implementation = rclpy_implementation if implementation is None else implementation
+ return isinstance(implementation, ImplementationPlaceholder)
diff --git a/rclpy/rclpy/impl/object_proxy.py b/rclpy/rclpy/impl/object_proxy.py
new file mode 100644
index 000000000..d8ca55275
--- /dev/null
+++ b/rclpy/rclpy/impl/object_proxy.py
@@ -0,0 +1,165 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Provides an Object proxy that allows the underlying class to change."""
+
+import ast
+import inspect
+
+# Inspired from:
+# https://pypi.python.org/pypi/ProxyTypes (PSF/ZPL license)
+
+
+class AbstractProxy(object):
+ """Delegates all operations (except .__actual__) to another object."""
+
+ __slots__ = ()
+
+ def __call__(self, *args, **kw):
+ return self.__actual__(*args, **kw)
+
+ def __getattribute__(self, attr):
+ subject = object.__getattribute__(self, '__actual__')
+ if attr == '__actual__':
+ return subject
+ return getattr(subject, attr)
+
+ def __setattr__(self, attr, val):
+ if attr == '__actual__':
+ object.__setattr__(self, attr, val)
+ else:
+ setattr(self.__actual__, attr, val)
+
+ def __delattr__(self, attr):
+ if attr == '__actual__':
+ object.__delattr__(self, attr)
+ else:
+ delattr(self.__actual__, attr)
+
+ def __nonzero__(self):
+ return bool(self.__actual__)
+
+ def __getitem__(self, arg):
+ return self.__actual__[arg]
+
+ def __setitem__(self, arg, val):
+ self.__actual__[arg] = val
+
+ def __delitem__(self, arg):
+ del self.__actual__[arg]
+
+ def __getslice__(self, i, j):
+ return self.__actual__[i:j]
+
+ def __setslice__(self, i, j, val):
+ self.__actual__[i:j] = val
+
+ def __delslice__(self, i, j):
+ del self.__actual__[i:j]
+
+ def __contains__(self, ob):
+ return ob in self.__actual__
+
+ for name in [
+ 'repr', 'str', 'hash', 'len', 'abs', 'complex', 'int', 'long', 'float',
+ 'iter', 'oct', 'hex'
+ ]:
+ code_object = compile(
+ 'def __{0}__(self): return {0}(self.__actual__)'.format(name),
+ __file__,
+ 'exec',
+ ast.PyCF_ONLY_AST,
+ )
+ ast.increment_lineno(code_object, inspect.getframeinfo(inspect.currentframe()).lineno - 6)
+ exec(compile(code_object, __file__, 'exec'))
+
+ for name in ['cmp', 'coerce', 'divmod']:
+ code_object = compile(
+ 'def __{0}__(self, ob): return {0}(self.__actual__, ob)'.format(name),
+ __file__,
+ 'exec',
+ ast.PyCF_ONLY_AST,
+ )
+ ast.increment_lineno(code_object, inspect.getframeinfo(inspect.currentframe()).lineno - 6)
+ exec(compile(code_object, __file__, 'exec'))
+
+ for name, op in [
+ ('lt', '<'), ('gt', '>'), ('le', '<='), ('ge', '>='), ('eq', '=='), ('ne', '!=')
+ ]:
+ code_object = compile(
+ 'def __{0}__(self, ob): return self.__actual__ {1} ob'.format(name, op),
+ __file__,
+ 'exec',
+ ast.PyCF_ONLY_AST,
+ )
+ ast.increment_lineno(code_object, inspect.getframeinfo(inspect.currentframe()).lineno - 6)
+ exec(compile(code_object, __file__, 'exec'))
+
+ for name, op in [('neg', '-'), ('pos', '+'), ('invert', '~')]:
+ code_object = compile(
+ 'def __{0}__(self): return {1} self.__actual__'.format(name, op),
+ __file__,
+ 'exec',
+ ast.PyCF_ONLY_AST,
+ )
+ ast.increment_lineno(code_object, inspect.getframeinfo(inspect.currentframe()).lineno - 6)
+ exec(compile(code_object, __file__, 'exec'))
+
+ for name, op in [
+ ('or', '|'), ('and', '&'), ('xor', '^'), ('lshift', '<<'), ('rshift', '>>'),
+ ('add', '+'), ('sub', '-'), ('mul', '*'), ('div', '/'), ('mod', '%'),
+ ('truediv', '/'), ('floordiv', '//')
+ ]:
+ code_object = compile(
+ """
+def __{name}__(self,ob):
+ return self.__actual__ {op} ob
+
+def __r{name}__(self,ob):
+ return ob {op} self.__actual__
+
+def __i{name}__(self,ob):
+ self.__actual__ {op}=ob
+ return self
+""".format(name=name, op=op),
+ __file__,
+ 'exec',
+ ast.PyCF_ONLY_AST,
+ )
+ ast.increment_lineno(code_object, inspect.getframeinfo(inspect.currentframe()).lineno - 6)
+ exec(compile(code_object, __file__, 'exec'))
+
+ del name, op
+
+ def __rdivmod__(self, ob):
+ return divmod(ob, self.__actual__)
+
+ def __pow__(self, *args):
+ return pow(self.__actual__, *args)
+
+ def __ipow__(self, ob):
+ self.__actual__ **= ob
+ return self
+
+ def __rpow__(self, ob):
+ return pow(ob, self.__actual__)
+
+
+class ObjectProxy(AbstractProxy):
+ """Proxy for an internally stored object."""
+
+ __slots__ = '__actual__'
+
+ def __init__(self, subject):
+ self.__actual__ = subject
diff --git a/rclpy/rclpy/impl/rmw_implementation_tools.py b/rclpy/rclpy/impl/rmw_implementation_tools.py
new file mode 100644
index 000000000..814b6c95d
--- /dev/null
+++ b/rclpy/rclpy/impl/rmw_implementation_tools.py
@@ -0,0 +1,86 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import importlib
+import logging
+
+import ament_index_python
+
+from rclpy.exceptions import ImplementationAlreadyImportedException
+from rclpy.exceptions import InvalidRCLPYImplementation
+
+__rmw_implementations = None
+__selected_rmw_implementation = None
+__rmw_implementation_module = None
+
+
+def reload_rmw_implementations():
+ """(Re)Load the available rmw implementations by inspecting the ament index."""
+ global __rmw_implementations
+ __rmw_implementations = sorted(ament_index_python.get_resources('rmw_implementation').keys())
+ return __rmw_implementations
+
+
+def get_rmw_implementations():
+ """Return a list of available rmw implementations by name."""
+ if __rmw_implementations is None:
+ reload_rmw_implementations()
+ return __rmw_implementations
+
+
+def select_rmw_implementation(rmw_implementation):
+ """Set the rmw implementation to be imported by name.
+
+ Can be called multiple times, but only before calling :py:func:`rclpy.init`
+ and/or :py:func:`import_rmw_implementation`.
+
+ If given an rmw implementation that is not in the list provided by
+ :py:func:`get_rmw_implementations` then the
+ :py:class:`InvalidRCLPYImplementation` exception will be raised.
+
+ :raises: :py:class:`ImplementationAlreadyImportedException` if
+ :py:func:`import_rmw_implementation` has already been called and the
+ module already imported.
+ """
+ global __selected_rmw_implementation
+ if __rmw_implementation_module is not None:
+ raise ImplementationAlreadyImportedException()
+ if rmw_implementation not in get_rmw_implementations():
+ raise InvalidRCLPYImplementation()
+ __selected_rmw_implementation = rmw_implementation
+
+
+def import_rmw_implementation():
+ """Import and return the selected or default rmw implementation.
+
+ This function can be called multiple times, but the rmw implementation is
+ only imported the first time.
+ Subsequent calls will return a cached rmw implementation module.
+
+ If :py:func:`select_rmw_implementation` has not previously been called then
+ a default implementation will be selected implicitly before loading.
+ """
+ global __rmw_implementation_module
+ if __rmw_implementation_module is not None:
+ return __rmw_implementation_module
+ if __selected_rmw_implementation is None:
+ logger = logging.getLogger('rclpy')
+ select_rmw_implementation(get_rmw_implementations()[0]) # select the first one
+ logger.debug("Implicitly selecting the '{0}' rmw implementation."
+ .format(__selected_rmw_implementation))
+ module_name = '._rclpy__{rmw_implementation}'.format(
+ rmw_implementation=__selected_rmw_implementation,
+ )
+ __rmw_implementation_module = importlib.import_module(module_name, package='rclpy')
+ return __rmw_implementation_module
diff --git a/rclpy/rclpy/selector.py b/rclpy/rclpy/selector.py
deleted file mode 100644
index 3adf5eae4..000000000
--- a/rclpy/rclpy/selector.py
+++ /dev/null
@@ -1,35 +0,0 @@
-# Copyright 2016 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import importlib
-
-import ament_index_python
-
-rmw_implementations = sorted(ament_index_python.get_resources('rmw_implementation').keys())
-_rmw_implementation = None
-
-
-def select(rmw_implementation):
- global _rmw_implementation
- _rmw_implementation = rmw_implementation
-
-
-def import_rmw_implementation():
- if _rmw_implementation is None:
- global _rmw_implementation
- _rmw_implementation = rmw_implementations[0]
- module_name = '._rclpy__{rmw_implementation}'.format(
- rmw_implementation=_rmw_implementation,
- )
- return importlib.import_module(module_name, package='rclpy')
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 5433d19f4..48f84aa49 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -169,7 +169,7 @@ rclpy_create_subscription(PyObject * Py_UNUSED(self), PyObject * args)
}
static PyObject *
-rclpy_get_implementation_identifier(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args))
+rclpy_get_rmw_implementation_identifier(PyObject * Py_UNUSED(self), PyObject * Py_UNUSED(args))
{
const char * rmw_implementation_identifier = rmw_get_implementation_identifier();
@@ -398,7 +398,7 @@ static PyMethodDef rclpy_methods[] = {
{"rclpy_shutdown", rclpy_shutdown, METH_NOARGS,
"rclpy_shutdown."},
- {"rclpy_get_implementation_identifier", rclpy_get_implementation_identifier,
+ {"rclpy_get_rmw_implementation_identifier", rclpy_get_rmw_implementation_identifier,
METH_NOARGS, "Retrieve the identifier for the active RMW implementation."},
{NULL, NULL, 0, NULL} /* sentinel */
};
diff --git a/rclpy/test/__init__.py b/rclpy/test/__init__.py
new file mode 100644
index 000000000..22c381697
--- /dev/null
+++ b/rclpy/test/__init__.py
@@ -0,0 +1,22 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+
+assert 'rclpy' not in sys.modules, 'rclpy should not have been imported before running tests'
+sys.path.insert(0, os.getcwd())
+
+import rclpy # noqa
+assert rclpy # silence pyflakes
diff --git a/rclpy/test/talker.py b/rclpy/test/talker.py
new file mode 100644
index 000000000..1fb99a276
--- /dev/null
+++ b/rclpy/test/talker.py
@@ -0,0 +1,53 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+import time
+
+# this is needed to allow rclpy to be imported from the build folder
+sys.path.insert(0, os.getcwd())
+
+
+def talker():
+ import rclpy
+ from rclpy.qos import qos_profile_default
+
+ rclpy.init([])
+
+ from std_msgs.msg import String
+
+ node = rclpy.create_node('talker')
+
+ chatter_pub = node.create_publisher(String, 'chatter', qos_profile_default)
+
+ msg = String()
+
+ i = 1
+ print('talker: beginning loop')
+ while True:
+ msg.data = 'Hello World: {0}'.format(i)
+ i += 1
+ print('talker sending: ' + msg.data)
+ chatter_pub.publish(msg)
+ time.sleep(1)
+
+if __name__ == '__main__':
+ try:
+ talker()
+ except KeyboardInterrupt:
+ print('talker stopped cleanly')
+ except BaseException:
+ print('exception in talker:', file=sys.stderr)
+ raise
diff --git a/rclpy/test/test_select_rmw_implementation.py b/rclpy/test/test_select_rmw_implementation.py
new file mode 100644
index 000000000..a02484fe2
--- /dev/null
+++ b/rclpy/test/test_select_rmw_implementation.py
@@ -0,0 +1,138 @@
+# Copyright 2016 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import ctypes
+import multiprocessing
+import os
+import sys
+import traceback
+
+# TODO(wjwwood): remove this once we get all the identifiers consistent
+identifiers_map = {
+ 'connext_static': 'rmw_connext_cpp',
+ 'opensplice_static': 'rmw_opensplice_cpp'
+}
+
+
+def run_catch_report_raise(func, *args, **kwargs):
+ try:
+ return func(*args, **kwargs)
+ except:
+ print('exception in {0}():'.format(func.__name__), file=sys.stderr)
+ traceback.print_exc()
+ raise
+
+
+def func_import_each_available_rmw_implementation(rmw_implementation):
+ import rclpy
+ from rclpy.impl.rmw_implementation_tools import get_rmw_implementations
+ from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
+
+ rmw_implementations = get_rmw_implementations()
+ assert(rmw_implementation in rmw_implementations)
+
+ select_rmw_implementation(rmw_implementation)
+ try:
+ rclpy.init([])
+ except ImportError as exc:
+ if "No module named 'rclpy._rclpy__" in '{0}'.format(exc):
+ # Not all rmw implementations generate a Python implementation.
+ # TODO(wjwwood): prune get_rmw_implementations by
+ # implementations that have generated a Python implementation
+ # (probably use a separate ament index entry).
+ print("Failed to import rmw implementation '{0}', ignoring."
+ .format(rmw_implementation), file=sys.stderr)
+ return ctypes.c_bool(True)
+ raise
+
+ set_rmw = rclpy.get_rmw_implementation_identifier()
+
+ assert set_rmw in identifiers_map, \
+ "expected identifier '{0}' to be in the map '{1}'".format(set_rmw, identifiers_map)
+
+ set_rmw_equivalent = identifiers_map[set_rmw]
+ assert set_rmw_equivalent == rmw_implementation, \
+ "expected '{0}' but got '{1}' (aka '{2}')".format(
+ rmw_implementation, set_rmw_equivalent, set_rmw)
+ return ctypes.c_bool(True)
+
+
+def test_import_each_available_rmw_implementation():
+ from rclpy.impl.rmw_implementation_tools import get_rmw_implementations
+ for rmw_implementation in get_rmw_implementations():
+ pool = multiprocessing.Pool(1)
+ result = pool.apply(
+ func=run_catch_report_raise,
+ args=(func_import_each_available_rmw_implementation, rmw_implementation,)
+ )
+
+ assert result
+ pool.close()
+ pool.join()
+
+
+def func_select_rmw_implementation_by_environment(rmw_implementation):
+ import rclpy
+ from rclpy.impl.rmw_implementation_tools import get_rmw_implementations
+
+ os.environ['RCLPY_IMPLEMENTATION'] = rmw_implementation
+
+ assert('RCLPY_IMPLEMENTATION' in os.environ)
+
+ rmw_implementations = get_rmw_implementations()
+ assert(os.environ['RCLPY_IMPLEMENTATION'] in rmw_implementations)
+
+ try:
+ rclpy.init([])
+ except ImportError as exc:
+ if "No module named 'rclpy._rclpy__" in '{0}'.format(exc):
+ # Not all rmw implementations generate a Python implementation.
+ # TODO(wjwwood): prune get_rmw_implementations by
+ # implementations that have generated a Python implementation
+ # (probably use a separate ament index entry).
+ return ctypes.c_bool(True)
+ print("Failed to import rmw implementation '{0}', ignoring."
+ .format(rmw_implementation), file=sys.stderr)
+ raise
+
+ set_rmw = rclpy.get_rmw_implementation_identifier()
+
+ assert set_rmw in identifiers_map, \
+ "expected identifier '{0}' to be in the map '{1}'".format(set_rmw, identifiers_map)
+
+ set_rmw_equivalent = identifiers_map[set_rmw]
+ assert set_rmw_equivalent == rmw_implementation, \
+ "expected '{0}' but got '{1}' (aka '{2}')".format(
+ rmw_implementation, set_rmw_equivalent, set_rmw)
+ return ctypes.c_bool(True)
+
+
+def test_select_rmw_implementation_by_environment():
+ from rclpy.impl.rmw_implementation_tools import get_rmw_implementations
+ orig_rclpy_implementation_env = os.environ.get('RCLPY_IMPLEMENTATION', None)
+ for rmw_implementation in get_rmw_implementations():
+ pool = multiprocessing.Pool(1)
+ result = pool.apply(
+ func=run_catch_report_raise,
+ args=(func_select_rmw_implementation_by_environment, rmw_implementation,)
+ )
+ if orig_rclpy_implementation_env is None:
+ if 'RCLPY_IMPLEMENTATION' in os.environ:
+ del os.environ['RCLPY_IMPLEMENTATION']
+ else:
+ os.environ['RCLPY_IMPLEMENTATION'] = orig_rclpy_implementation_env
+
+ assert result, "type('{0}'): '{1}'".format(type(result), result)
+ pool.close()
+ pool.join()
diff --git a/rclpy/test/test_switch_rmw_implementation.py b/rclpy/test/test_switch_rmw_implementation.py
deleted file mode 100644
index e006379c1..000000000
--- a/rclpy/test/test_switch_rmw_implementation.py
+++ /dev/null
@@ -1,101 +0,0 @@
-# Copyright 2016 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import importlib
-from multiprocessing import Array
-from multiprocessing import Process
-import os
-
-
-def f_rmw(result):
- import rclpy
- import rclpy.selector
- if len(rclpy.selector.rmw_implementations) < 2:
- # Ignore this test, we need at least two implementations
- print('Need more than one rmw implementation installed')
- return
-
- for second_rmw in rclpy.selector.rmw_implementations[1:]:
- try:
- rmw_implementation = importlib.import_module(
- '._rclpy__{rmw_implementation}'.format(rmw_implementation=second_rmw),
- package='rclpy').rclpy_get_implementation_identifier()
- break
- except ImportError:
- pass
-
- orig_rmw = rclpy._rclpy
-
- rclpy.selector.select(second_rmw)
-
- rclpy.init([])
-
- set_rmw = rclpy._rclpy.rclpy_get_implementation_identifier()
-
- result[0] = (set_rmw == rmw_implementation)
- result[1] = (set_rmw != orig_rmw)
-
-
-def f_rmw_env(result):
- import rclpy
- import rclpy.selector
- if len(rclpy.selector.rmw_implementations) < 2:
- # Ignore this test, we need at least two implementations
- print('Need more than one rmw implementation installed')
- return
-
- for second_rmw in rclpy.selector.rmw_implementations[1:]:
- try:
- rmw_implementation = importlib.import_module(
- '._rclpy__{rmw_implementation}'.format(rmw_implementation=second_rmw),
- package='rclpy').rclpy_get_implementation_identifier()
- break
- except ImportError:
- pass
-
- orig_rmw = rclpy._rclpy
-
- old_rclpy_rmw_env = os.environ.get('RCLPY_IMPLEMENTATION')
- os.environ['RCLPY_IMPLEMENTATION'] = second_rmw
-
- rclpy.init([])
-
- set_rmw = rclpy._rclpy.rclpy_get_implementation_identifier()
-
- result[0] = (set_rmw == rmw_implementation)
- result[1] = (set_rmw != orig_rmw)
- if old_rclpy_rmw_env is None:
- del os.environ['RCLPY_IMPLEMENTATION']
- else:
- os.environ['RCLPY_IMPLEMENTATION'] = old_rclpy_rmw_env
-
-
-def test_switch_rmw():
- result = Array('b', [-1 for _ in range(2)])
- test_process = Process(target=f_rmw, args=[result])
- test_process.start()
- test_process.join()
-
- assert result[0] == 1
- assert result[1] == 1
-
-
-def test_switch_rmw_env():
- result = Array('b', [-1 for _ in range(2)])
- test_process = Process(target=f_rmw_env, args=[result])
- test_process.start()
- test_process.join()
-
- assert result[0] == 1
- assert result[1] == 1
diff --git a/rclpy/test/test_talker_listener.py b/rclpy/test/test_talker_listener.py
index 0738d88c7..b53946fe9 100644
--- a/rclpy/test/test_talker_listener.py
+++ b/rclpy/test/test_talker_listener.py
@@ -12,64 +12,57 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-from functools import partial
-from multiprocessing import Process
-from time import sleep
+import functools
+import os
+import sys
+from launch import LaunchDescriptor
+from launch.launcher import AsynchronousLauncher
+from launch.launcher import DefaultLauncher
-def talker():
- import rclpy
- rclpy.init([])
-
- from rclpy.qos import qos_profile_default
-
- from std_msgs.msg import String
- assert String.__class__._TYPE_SUPPORT is not None
-
- node = rclpy.create_node('talker')
-
- chatter_pub = node.create_publisher(String, 'chatter', qos_profile_default)
-
- msg = String()
+this_dir = os.path.normpath(os.path.dirname(os.path.abspath(__file__)))
- i = 1
- while True:
- msg.data = 'Hello World: {0}'.format(i)
- i += 1
- chatter_pub.publish(msg)
- sleep(1)
-
-def listener_callback(msg, talker_process, received_messages):
- import rclpy
- talker_process.terminate()
+def listener_callback(msg, received_messages):
+ print('received: ' + msg.data)
received_messages.append(msg)
- rclpy.shutdown()
-def test_rclpy_talker_listener():
- talker_process = Process(target=talker)
- talker_process.start()
+def test_talker_listener():
+ launch_desc = LaunchDescriptor()
+ launch_desc.add_process(
+ cmd=[sys.executable, os.path.join(this_dir, 'talker.py')],
+ name='test_talker_listener__talker'
+ )
+ launcher = DefaultLauncher()
+ launcher.add_launch_descriptor(launch_desc)
+ async_launcher = AsynchronousLauncher(launcher)
+ async_launcher.start()
import rclpy
+
rclpy.init([])
from rclpy.qos import qos_profile_default
+ from std_msgs.msg import String
+ assert String.__class__._TYPE_SUPPORT is not None
+
node = rclpy.create_node('listener')
received_messages = []
- chatter_callback = partial(
- listener_callback, talker_process=talker_process, received_messages=received_messages)
-
- from std_msgs.msg import String
- assert String.__class__._TYPE_SUPPORT is not None
-
+ chatter_callback = functools.partial(
+ listener_callback, received_messages=received_messages)
+ # TODO(wjwwood): should the subscription object hang around?
node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
- rclpy.spin(node)
+ spin_count = 0
+ while len(received_messages) == 0 and spin_count < 10 and launcher.is_launch_running():
+ rclpy.spin_once(node) # This will wait 1 second or until something has been done.
+ spin_count += 1
- talker_process.join()
+ async_launcher.terminate()
+ async_launcher.join()
assert len(received_messages) > 0, "Should have received a message from talker"