[WIP] Futures/coroutines for services

rclpy/CMakeLists.txt

@@ -70,10 +70,32 @@ function(configure_python_c_extension_library _library_name)
   )
 endfunction()
 
+# Library with signal handler
+add_library(
+  rclpy_sigint
+  SHARED src/rclpy/sigint_gc.c
+)
+target_include_directories(rclpy_sigint PUBLIC ${CMAKE_SOURCE_DIR})
+install(
+  TARGETS rclpy_sigint
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+ament_target_dependencies(rclpy_sigint
+  "rcl"
+)
+# Export so other packages can load this library when importing rclpy
+ament_export_libraries(rclpy_sigint)
+# Windows dllimport/dllexport
+target_compile_definitions(rclpy_sigint PRIVATE "RCLPY_SIGINT_BUILDING_LIBRARY")
+
+# Main library
 add_library(
   rclpy
   SHARED src/rclpy/_rclpy.c
 )
+target_link_libraries(rclpy rclpy_sigint)
 configure_python_c_extension_library(rclpy)
 ament_target_dependencies(rclpy
   "rcl"
@@ -90,6 +112,18 @@ ament_target_dependencies(rclpy_logging
   "rcutils"
 )
 
+# WaitSet wrapper library
+add_library(
+  rclpy_wait_set
+  SHARED src/rclpy/_rclpy_wait_set.c
+)
+target_link_libraries(rclpy_wait_set rclpy_sigint)
+configure_python_c_extension_library(rclpy_wait_set)
+ament_target_dependencies(rclpy_wait_set
+  "rcl"
+  "rcutils"
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
@@ -109,6 +143,7 @@ if(BUILD_TESTING)
       PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}"
       APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}
         PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      APPEND_LIBRARY_DIRS "$<TARGET_FILE_DIR:rclpy_sigint>"  # Windows add rclpy_sigint.dll to path
       TIMEOUT 90
     )
   endif()

rclpy/rclpy/callback_groups.py

@@ -45,6 +45,9 @@ def can_execute(self, entity):
         """
         Return true if an entity can be executed.
 
+        The executor may call this on a task that has already started execution. In this case the
+        callback group should return True from this method.
+
         :param entity: a subscription, timer, client, or service instance
         :rtype: bool
         """
@@ -96,17 +99,19 @@ def __init__(self):
     def can_execute(self, entity):
         with self._lock:
             assert weakref.ref(entity) in self.entities
-            return self._active_entity is None
+            return self._active_entity is None or weakref.ref(entity) == self._active_entity
 
     def beginning_execution(self, entity):
         with self._lock:
-            assert weakref.ref(entity) in self.entities
+            weak_entity = weakref.ref(entity)
+            assert weak_entity in self.entities
             if self._active_entity is None:
-                self._active_entity = entity
+                self._active_entity = weak_entity
                 return True
         return False
 
     def ending_execution(self, entity):
         with self._lock:
-            assert self._active_entity == entity
+            weak_entity = weakref.ref(entity)
+            assert self._active_entity == weak_entity
             self._active_entity = None

rclpy/rclpy/client.py

@@ -14,7 +14,9 @@
 
 import threading
 
+from rclpy.executor_handle import ExecutorHandle
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.impl.implementation_singleton import rclpy_wait_set_implementation as _rclpy_wait_set
 import rclpy.utilities
 
 
@@ -23,24 +25,19 @@ class ResponseThread(threading.Thread):
     def __init__(self, client):
         threading.Thread.__init__(self)
         self.client = client
-        self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set()
-        _rclpy.rclpy_wait_set_init(self.wait_set, 0, 1, 0, 1, 0)
-        _rclpy.rclpy_wait_set_clear_entities('client', self.wait_set)
-        _rclpy.rclpy_wait_set_add_entity(
-            'client', self.wait_set, self.client.client_handle)
+        self._wait_set = _rclpy_wait_set.WaitSet()
 
     def run(self):
         [sigint_gc, sigint_gc_handle] = _rclpy.rclpy_get_sigint_guard_condition()
-        _rclpy.rclpy_wait_set_add_entity('guard_condition', self.wait_set, sigint_gc)
+        self._wait_set.clear()
+        self._wait_set.add_guard_conditions([sigint_gc])
+        self._wait_set.add_clients([self.client.client_handle])
 
-        _rclpy.rclpy_wait(self.wait_set, -1)
-
-        guard_condition_ready_list = \
-            _rclpy.rclpy_get_ready_entities('guard_condition', self.wait_set)
+        self._wait_set.wait(-1)
 
         # destroying here to make sure we dont call shutdown before cleaning up
         _rclpy.rclpy_destroy_entity(sigint_gc)
-        if sigint_gc_handle in guard_condition_ready_list:
+        if self._wait_set.is_ready(sigint_gc):
             rclpy.utilities.shutdown()
             return
         response = _rclpy.rclpy_take_response(
@@ -64,8 +61,20 @@ def __init__(
         self.sequence_number = 0
         self.response = None
         self.callback_group = callback_group
-        # True when the callback is ready to fire but has not been "taken" by an executor
-        self._executor_event = False
+        # Holds info the executor uses to do work for this entity
+        self._executor_handle = ExecutorHandle(self._take, self._execute)
+
+    def _take(self):
+        response = _rclpy.rclpy_take_response(
+            self.client_handle, self.srv_type.Response, self.sequence_number)
+        return response
+
+    def _execute(self, response):
+        if response:
+            # clients spawn their own thread to wait for a response in the
+            # wait_for_future function. Users can either use this mechanism or monitor
+            # the content of client.response to check if a response has been received
+            self.response = response
 
     def call(self, req):
         self.response = None
@@ -77,3 +86,6 @@ def wait_for_future(self):
         thread1 = ResponseThread(self)
         thread1.start()
         thread1.join()
+
+    def service_is_ready(self):
+        return _rclpy.rclpy_service_server_is_available(self.node_handle, self.client_handle)

rclpy/rclpy/executor_handle.py

@@ -0,0 +1,49 @@
+# Copyright 2017 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 queue
+
+import rclpy.future
+from rclpy.utilities import timeout_sec_to_nsec
+
+
+class ExecutorHandle:
+    """Interface between an entity and an executor."""
+
+    def __init__(self, take_callback, execute_callback, cancel_ready_callback=None):
+        self._take_from_wait_list = take_callback
+        self.execute_callback = execute_callback
+        self._cancel_ready_callback = cancel_ready_callback
+        # True when the callback is ready to fire but has not been "taken" by an executor
+        self._ready = False
+
+    def notify_ready(self):
+        """Receive notification from executor that this entity was ready in the wait list."""
+        self._ready = True
+
+    def cancel_ready(self):
+        """Receive notification from executor that this entity could not be taken."""
+        self._ready = False
+        # Hook for guard conditions to retrigger themselves
+        if self._cancel_ready_callback:
+            self._cancel_ready_callback
+
+    def ready(self):
+        return self._ready
+
+    def take_from_wait_list(self):
+        """Get data from rcl."""
+        args = self._take_from_wait_list()
+        self._ready = False
+        return args