1515from inspect import ismethod
1616import sys
1717import threading
18- from types import TracebackType
18+ from types import MethodType , TracebackType
1919from typing import Callable
2020from typing import ContextManager
2121from typing import List
2222from typing import Optional
23+ from typing import Protocol
2324from typing import Type
24- import weakref
25+ from typing import Union
26+ from weakref import WeakMethod
27+
28+ from rclpy .destroyable import DestroyableType
29+
30+
31+ class ContextHandle (DestroyableType , Protocol ):
32+
33+ def ok (self ) -> bool :
34+ ...
35+
36+ def get_domain_id (self ) -> int :
37+ ...
38+
39+ def shutdown (self ) -> None :
40+ ...
2541
2642
2743g_logging_configure_lock = threading .Lock ()
28- g_logging_ref_count = 0
44+ g_logging_ref_count : int = 0
2945
3046
3147class Context (ContextManager ['Context' ]):
@@ -43,24 +59,25 @@ class Context(ContextManager['Context']):
4359
4460 """
4561
46- def __init__ (self ):
62+ def __init__ (self ) -> None :
4763 self ._lock = threading .Lock ()
48- self ._callbacks = []
64+ self ._callbacks : List [ Union [ 'WeakMethod[MethodType]' , Callable [[], None ]]] = []
4965 self ._logging_initialized = False
50- self .__context = None
66+ self .__context : Optional [ ContextHandle ] = None
5167
5268 @property
53- def handle (self ):
69+ def handle (self ) -> Optional [ ContextHandle ] :
5470 return self .__context
5571
56- def destroy (self ):
57- self .__context .destroy_when_not_in_use ()
72+ def destroy (self ) -> None :
73+ if self .__context :
74+ self .__context .destroy_when_not_in_use ()
5875
5976 def init (self ,
6077 args : Optional [List [str ]] = None ,
6178 * ,
6279 initialize_logging : bool = True ,
63- domain_id : Optional [int ] = None ):
80+ domain_id : Optional [int ] = None ) -> None :
6481 """
6582 Initialize ROS communications for a given context.
6683
@@ -89,22 +106,22 @@ def init(self,
89106 _rclpy .rclpy_logging_configure (self .__context )
90107 self ._logging_initialized = True
91108
92- def ok (self ):
109+ def ok (self ) -> bool :
93110 """Check if context hasn't been shut down."""
94111 with self ._lock :
95112 if self .__context is None :
96113 return False
97114 with self .__context :
98115 return self .__context .ok ()
99116
100- def _call_on_shutdown_callbacks (self ):
117+ def _call_on_shutdown_callbacks (self ) -> None :
101118 for weak_method in self ._callbacks :
102119 callback = weak_method ()
103120 if callback is not None :
104121 callback ()
105122 self ._callbacks = []
106123
107- def shutdown (self ):
124+ def shutdown (self ) -> None :
108125 """Shutdown this context."""
109126 if self .__context is None :
110127 raise RuntimeError ('Context must be initialized before it can be shutdown' )
@@ -113,7 +130,7 @@ def shutdown(self):
113130 self ._call_on_shutdown_callbacks ()
114131 self ._logging_fini ()
115132
116- def try_shutdown (self ):
133+ def try_shutdown (self ) -> None :
117134 """Shutdown this context, if not already shutdown."""
118135 if self .__context is None :
119136 return
@@ -123,23 +140,32 @@ def try_shutdown(self):
123140 self ._call_on_shutdown_callbacks ()
124141 self ._logging_fini ()
125142
126- def _remove_callback (self , weak_method ) :
143+ def _remove_callback (self , weak_method : 'WeakMethod[MethodType]' ) -> None :
127144 self ._callbacks .remove (weak_method )
128145
129- def on_shutdown (self , callback : Callable [[], None ]):
146+ def on_shutdown (self , callback : Callable [[], None ]) -> None :
130147 """Add a callback to be called on shutdown."""
131148 if not callable (callback ):
132149 raise TypeError ('callback should be a callable, got {}' , type (callback ))
150+
151+ if self .__context is None :
152+ with self ._lock :
153+ if ismethod (callback ):
154+ self ._callbacks .append (WeakMethod (callback , self ._remove_callback ))
155+ else :
156+ self ._callbacks .append (callback )
157+ return
158+
133159 with self .__context , self ._lock :
134160 if not self .__context .ok ():
135161 callback ()
136162 else :
137163 if ismethod (callback ):
138- self ._callbacks .append (weakref . WeakMethod (callback , self ._remove_callback ))
164+ self ._callbacks .append (WeakMethod (callback , self ._remove_callback ))
139165 else :
140166 self ._callbacks .append (callback )
141167
142- def _logging_fini (self ):
168+ def _logging_fini (self ) -> None :
143169 # This function must be called with self._lock held.
144170 from rclpy .impl .implementation_singleton import rclpy_implementation
145171 global g_logging_ref_count
@@ -153,7 +179,7 @@ def _logging_fini(self):
153179 'Unexpected error: logger ref count should never be lower that zero' )
154180 self ._logging_initialized = False
155181
156- def get_domain_id (self ):
182+ def get_domain_id (self ) -> int :
157183 """Get domain id of context."""
158184 if self .__context is None :
159185 raise RuntimeError ('Context must be initialized before it can have a domain id' )
@@ -162,7 +188,7 @@ def get_domain_id(self):
162188
163189 def __enter__ (self ) -> 'Context' :
164190 # We do not accept parameters here. If one wants to customize the init() call,
165- # they would have to call it manaully and not use the ContextManager convenience
191+ # they would have to call it manually and not use the ContextManager convenience
166192 self .init ()
167193 return self
168194
0 commit comments