1212# See the License for the specific language governing permissions and
1313# limitations under the License.
1414
15+ from typing import List , Optional , Set , TYPE_CHECKING
1516import weakref
1617
1718from rcl_interfaces .msg import SetParametersResult
2324from rclpy .time import Time
2425import rosgraph_msgs .msg
2526
27+ if TYPE_CHECKING :
28+ from rclpy .node import Node
29+ from rclpy .subscription import Subscription
30+
2631CLOCK_TOPIC = '/clock'
2732USE_SIM_TIME_NAME = 'use_sim_time'
2833
2934
3035class TimeSource :
3136
32- def __init__ (self , * , node = None ):
33- self ._clock_sub = None
34- self ._node_weak_ref = None
35- self ._associated_clocks = set ()
37+ def __init__ (self , * , node : Optional [ 'Node' ] = None ):
38+ self ._clock_sub : Optional [ 'Subscription' ] = None
39+ self ._node_weak_ref : Optional [ weakref . ReferenceType [ 'Node' ]] = None
40+ self ._associated_clocks : Set [ ROSClock ] = set ()
3641 # Zero time is a special value that means time is uninitialzied
3742 self ._last_time_set = Time (clock_type = ClockType .ROS_TIME )
3843 self ._ros_time_is_active = False
3944 if node is not None :
4045 self .attach_node (node )
4146
4247 @property
43- def ros_time_is_active (self ):
48+ def ros_time_is_active (self ) -> bool :
4449 return self ._ros_time_is_active
4550
4651 @ros_time_is_active .setter
47- def ros_time_is_active (self , enabled ) :
52+ def ros_time_is_active (self , enabled : bool ) -> None :
4853 if self ._ros_time_is_active == enabled :
4954 return
5055 self ._ros_time_is_active = enabled
@@ -59,7 +64,7 @@ def ros_time_is_active(self, enabled):
5964 node .destroy_subscription (self ._clock_sub )
6065 self ._clock_sub = None
6166
62- def _subscribe_to_clock_topic (self ):
67+ def _subscribe_to_clock_topic (self ) -> None :
6368 if self ._clock_sub is None :
6469 node = self ._get_node ()
6570 if node is not None :
@@ -70,7 +75,7 @@ def _subscribe_to_clock_topic(self):
7075 QoSProfile (depth = 1 , reliability = ReliabilityPolicy .BEST_EFFORT )
7176 )
7277
73- def attach_node (self , node ) :
78+ def attach_node (self , node : 'Node' ) -> None :
7479 from rclpy .node import Node
7580 if not isinstance (node , Node ):
7681 raise TypeError ('Node must be of type rclpy.node.Node' )
@@ -97,7 +102,7 @@ def attach_node(self, node):
97102
98103 node .add_on_set_parameters_callback (self ._on_parameter_event )
99104
100- def detach_node (self ):
105+ def detach_node (self ) -> None :
101106 # Remove the subscription to the clock topic.
102107 if self ._clock_sub is not None :
103108 node = self ._get_node ()
@@ -107,22 +112,22 @@ def detach_node(self):
107112 self ._clock_sub = None
108113 self ._node_weak_ref = None
109114
110- def attach_clock (self , clock ) :
115+ def attach_clock (self , clock : ROSClock ) -> None :
111116 if not isinstance (clock , ROSClock ):
112117 raise ValueError ('Only clocks with type ROS_TIME can be attached.' )
113118
114119 clock .set_ros_time_override (self ._last_time_set )
115120 clock ._set_ros_time_is_active (self .ros_time_is_active )
116121 self ._associated_clocks .add (clock )
117122
118- def clock_callback (self , msg ) :
123+ def clock_callback (self , msg : rosgraph_msgs . msg . Clock ) -> None :
119124 # Cache the last message in case a new clock is attached.
120125 time_from_msg = Time .from_msg (msg .clock )
121126 self ._last_time_set = time_from_msg
122127 for clock in self ._associated_clocks :
123128 clock .set_ros_time_override (time_from_msg )
124129
125- def _on_parameter_event (self , parameter_list ) :
130+ def _on_parameter_event (self , parameter_list : List [ Parameter ]) -> SetParametersResult :
126131 successful = True
127132 reason = ''
128133
@@ -142,7 +147,7 @@ def _on_parameter_event(self, parameter_list):
142147
143148 return SetParametersResult (successful = successful , reason = reason )
144149
145- def _get_node (self ):
150+ def _get_node (self ) -> Optional [ 'Node' ] :
146151 if self ._node_weak_ref is not None :
147152 return self ._node_weak_ref ()
148153 return None
0 commit comments