Proof of concept part of cpython extension using Cython

rclpy/CMakeLists.txt

@@ -80,6 +80,45 @@ ament_target_dependencies(rclpy
   "rcutils"
 )
 
+# Generate code using cython
+set( CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/cmake )
+find_package(Cython REQUIRED)
+
+function(cythonize)
+  set(options)
+  set(oneValueArgs TARGET INPUT OUTPUT)
+  set(multiValueArgs DEPENDS)
+  cmake_parse_arguments(_ARG "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+  add_custom_command( OUTPUT ${_ARG_OUTPUT}
+    COMMAND ${CYTHON_EXECUTABLE}
+    ARGS -I rclpy/ --output-file ${_ARG_OUTPUT} ${_ARG_INPUT}
+    DEPENDS ${_ARG_INPUT} ${_ARG_DEPENDS}
+    COMMENT "Cython generating code to ${_ARG_OUTPUT}"
+  )
+  add_custom_target(${_ARG_TARGET} DEPENDS ${_ARG_OUTPUT})
+endfunction()
+
+set( _cython_output_dir "${CMAKE_CURRENT_BINARY_DIR}/cython_output")
+file(MAKE_DIRECTORY ${_cython_output_dir})
+cythonize(
+  TARGET cythonize_wait_set
+  INPUT "${CMAKE_CURRENT_SOURCE_DIR}/rclpy/_rclpy_wait_set.pyx"
+  OUTPUT "${_cython_output_dir}/rclpy_wait_set.c"
+  DEPENDS rclpy/rcl.pxd
+)
+
+add_library(
+  rclpy_wait_set
+  SHARED ${_cython_output_dir}/rclpy_wait_set.c
+)
+add_dependencies(rclpy_wait_set cythonize_wait_set)
+configure_python_c_extension_library(rclpy_wait_set)
+ament_target_dependencies(rclpy_wait_set
+  "rcl"
+  "rcutils"
+)
+
 # Logging support provided by rcutils
 add_library(
   rclpy_logging

rclpy/cmake/FindCython.cmake

@@ -0,0 +1,44 @@
+# Find the Cython compiler.
+#
+# This code sets the following variables:
+#
+#  CYTHON_EXECUTABLE
+#
+# See also UseCython.cmake
+
+#=============================================================================
+# Copyright 2011 Kitware, 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.
+#=============================================================================
+
+# Use the Cython executable that lives next to the Python executable
+# if it is a local installation.
+find_package( PythonInterp )
+if( PYTHONINTERP_FOUND )
+  get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH )
+  find_program( CYTHON_EXECUTABLE
+    NAMES cython cython.bat cython3
+    HINTS ${_python_path}
+    )
+else()
+  find_program( CYTHON_EXECUTABLE
+    NAMES cython cython.bat cython3
+    )
+endif()
+
+
+include( FindPackageHandleStandardArgs )
+FIND_PACKAGE_HANDLE_STANDARD_ARGS( Cython REQUIRED_VARS CYTHON_EXECUTABLE )
+
+mark_as_advanced( CYTHON_EXECUTABLE )

rclpy/rclpy/_rclpy_wait_set.pyx

@@ -0,0 +1,163 @@
+# Copyright 2018 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.
+
+from cpython.pycapsule cimport PyCapsule_GetPointer
+from cpython.pycapsule cimport PyCapsule_IsValid
+from rcl cimport *
+
+
+def raise_runtime_error():
+    raw_error = rcl_get_error_string_safe()
+    try:
+        error = 'Failed to wait on waitset: "%s"' % raw_error.decode('UTF-8', 'strict')
+    finally:
+        rcl_reset_error()
+
+
+cdef class WaitSet:
+    # List of PyCapsule containing rcl_subscription_t
+    cdef public list _pysubs
+    # List of PyCapsule containing rcl_guard_condition_t
+    cdef public list _pygcs
+    # List of PyCapsule containing rcl_timer_t
+    cdef public list _pytmrs
+    # List of PyCapsule containing rcl_client_t
+    cdef public list _pyclis
+    # List of PyCapsule containing rcl_service_t
+    cdef public list _pysrvs
+
+    def __init__(self):
+        self._pysubs = []
+        self._pygcs = []
+        self._pytmrs = []
+        self._pyclis = []
+        self._pysrvs = []
+
+    def add_subscription(self, entity):
+        self._pysubs.append(entity.subscription_handle)
+
+    def add_guard_condition(self, entity):
+        self._pygcs.append(entity.guard_handle)
+
+    def add_timer(self, entity):
+        self._pytmrs.append(entity.timer_handle)
+
+    def add_client(self, entity):
+        self._pyclis.append(entity.client_handle)
+
+    def add_service(self, entity):
+        self._pysrvs.append(entity.service_handle)
+
+    def wait(self, timeout=-1):
+        cdef rcl_ret_t ret
+        cdef bytes raw_error
+        cdef size_t ci
+        cdef void * entity_pointer
+        # coerce timeout from python type while GIL is held
+        cdef int ctimeout = timeout
+
+        cdef rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set()
+        try:
+            # Initialize the wait set
+            ret = rcl_wait_set_init(
+                &wait_set, len(self._pysubs), len(self._pygcs), len(self._pytmrs),
+                len(self._pyclis), len(self._pysrvs), rcl_get_default_allocator())
+            if ret != RCL_RET_OK:
+                raise_runtime_error()
+
+            # Populate wait set
+            for i, capsule in enumerate(self._pysubs):
+                wait_set.subscriptions[i] = PyCapsule_GetPointer(capsule, 'rcl_subscription_t')
+            for i, capsule in enumerate(self._pygcs):
+                wait_set.guard_conditions[i] = PyCapsule_GetPointer(
+                    capsule, 'rcl_guard_condition_t')
+            for i, capsule in enumerate(self._pytimers):
+                wait_set.timers[i] = PyCapsule_GetPointer(capsule, 'rcl_timer_t')
+            for i, capsule in enumerate(self._pyclis):
+                wait_set.clients[i] = PyCapsule_GetPointer(capsule, 'rcl_client_t')
+            for i, capsule in enumerate(self._pysrvs):
+                wait_set.services[i] = PyCapsule_GetPointer(capsule, 'rcl_service_t')
+
+            # Actually wait
+            with nogil:
+                ret = rcl_wait(&wait_set, ctimeout);
+            if ret != RCL_RET_OK or ret != RCL_RET_TIMEOUT:
+                raise_runtime_error()
+
+            # Make ready entities available to python code
+            ready_subscriptions = []
+            ready_guard_conditions = []
+            ready_timers = []
+            ready_clients = []
+            ready_services = []
+            if ret != RCL_RET_TIMEOUT:
+                for entity in self._pysubs:
+                    ci = 0
+                    while ci < wait_set.size_of_subscriptions:
+                        entity_pointer = PyCapsule_GetPointer(capsule, 'rcl_subscription_t')
+                        if wait_set.subscriptions[i] == entity_pointer:
+                            self.ready_subscriptions.append(entity)
+                        ci += 1
+                for entity in self._pygcs:
+                    ci = 0
+                    while ci < wait_set.size_of_guard_conditions:
+                        entity_pointer = PyCapsule_GetPointer(capsule, 'rcl_guard_condition_t')
+                        if wait_set.guard_conditions[i] == entity_pointer:
+                            self.ready_guard_conditions.append(entity)
+                        ci += 1
+                for entity in self._pytmrs:
+                    ci = 0
+                    while ci < wait_set.size_of_timers:
+                        entity_pointer = PyCapsule_GetPointer(capsule, 'rcl_timer_t')
+                        if wait_set.timers[i] == entity_pointer:
+                            self.ready_timers.append(entity)
+                        ci += 1
+                for entity in self._pyclis:
+                    ci = 0
+                    while ci < wait_set.size_of_clients:
+                        entity_pointer = PyCapsule_GetPointer(capsule, 'rcl_client_t')
+                        if wait_set.clients[i] == entity_pointer:
+                            self.ready_clients.append(entity)
+                        ci += 1
+                for entity in self._pysrvs:
+                    ci = 0
+                    while ci < wait_set.size_of_services:
+                        entity_pointer = PyCapsule_GetPointer(capsule, 'rcl_service_t')
+                        if wait_set.services[i] == entity_pointer:
+                            self.ready_services.append(entity)
+                        ci += 1
+            self._pysubs = ready_subscriptions
+            self._pygcs = ready_guard_conditions
+            self._pytmrs = ready_timers
+            self._pyclis = ready_clients
+            self._pysrvs = ready_services
+        finally:
+            ret = rcl_wait_set_fini(&(wait_set))
+            if ret != RCL_RET_OK:
+                raise_runtime_error()
+
+    def ready_subscriptions(self):
+        return self._pysubs
+
+    def ready_guard_conditions(self):
+        return self._pygcs
+
+    def ready_timers(self):
+        return self._pytmrs
+
+    def ready_clients(self):
+        return self._pyclis
+
+    def ready_services(self):
+        return self._pysrvs

rclpy/rclpy/impl/implementation_singleton.py

@@ -30,3 +30,4 @@
 
 rclpy_implementation = _import('._rclpy')
 rclpy_logging_implementation = _import('._rclpy_logging')
+rclpy_waitset = _import('._rclpy_wait_set')

rclpy/rclpy/rcl.pxd

@@ -0,0 +1,46 @@
+# Copyright 2018 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.
+
+cdef extern from "<rcl/rcl.h>":
+    # Let cython know what comes from this header with roughly correct types
+    ctypedef int rcl_ret_t
+    int RCL_RET_OK
+    int RCL_RET_TIMEOUT
+
+    struct rcl_wait_set_t:
+        int size_of_subscriptions
+        int size_of_guard_conditions
+        int size_of_timers
+        int size_of_clients
+        int size_of_services
+        void ** subscriptions
+        void ** guard_conditions
+        void ** timers
+        void ** clients
+        void ** services
+
+    struct rcl_allocator_t:
+        pass
+
+    rcl_allocator_t rcl_get_default_allocator() nogil;
+
+    rcl_ret_t rcl_wait(rcl_wait_set_t *, int timeout) nogil
+    rcl_ret_t rcl_wait_set_init(rcl_wait_set_t *, int, int, int, int, int, rcl_allocator_t) nogil;
+    rcl_ret_t rcl_wait_set_fini(rcl_wait_set_t *) nogil
+    rcl_wait_set_t rcl_get_zero_initialized_wait_set() nogil
+
+
+cdef extern from "<rcl/error_handling.h>":
+    const char * rcl_get_error_string_safe() nogil
+    void rcl_reset_error() nogil