Fix exception handling

rclpy/rclpy/executors.py

@@ -521,6 +521,8 @@ def spin_once(self, timeout_sec=None):
             pass
         else:
             handler()
+            if handler.exception() is not None:
+                raise handler.exception()
 
 
 class MultiThreadedExecutor(Executor):

rclpy/rclpy/task.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 import inspect
+import sys
 import threading
 import weakref
 
@@ -34,6 +35,7 @@ def __init__(self, *, executor=None):
         self._result = None
         # An exception raised by the handler when called
         self._exception = None
+        self._exception_fetched = False
         # callbacks to be scheduled after this task completes
         self._callbacks = []
         # Lock for threadsafety
@@ -42,6 +44,12 @@ def __init__(self, *, executor=None):
         self._executor = None
         self._set_executor(executor)
 
+    def __del__(self):
+        if self._exception is not None and not self._exception_fetched:
+            print(
+                'The following exception was never retrieved: ' +
+                str(self._exception), file=sys.stderr)
+
     def __await__(self):
         # Yield if the task is not finished
         while not self._done:
@@ -87,6 +95,7 @@ def exception(self):
 
         :return: The exception raised by the task
         """
+        self._exception_fetched = True
         return self._exception
 
     def set_result(self, result):
@@ -109,6 +118,7 @@ def set_exception(self, exception):
         """
         with self._lock:
             self._exception = exception
+            self._exception_fetched = False
             self._done = True
             self._cancelled = False
             self._schedule_done_callbacks()