1414
1515from typing import Callable
1616from typing import Iterable
17+ from typing import List
1718from typing import Optional
1819from typing import Text
1920from typing import Type
2829from rclpy .exceptions import ParameterAlreadyDeclaredException
2930from rclpy .parameter import Parameter
3031from rclpy .publisher import Publisher
32+ from rclpy .qos import QoSDurabilityPolicy
33+ from rclpy .qos import QoSHistoryPolicy
34+ from rclpy .qos import QoSLivelinessPolicy
3135from rclpy .qos import QoSPolicyKind
3236from rclpy .qos import QoSProfile
37+ from rclpy .qos import QoSReliabilityPolicy
3338from rclpy .subscription import Subscription
3439
3540if TYPE_CHECKING :
41+ from typing import TypeAlias
3642 from rclpy .node import Node
3743
3844
@@ -41,7 +47,7 @@ class InvalidQosOverridesError(Exception):
4147
4248
4349# Return type of qos validation callbacks
44- QosCallbackResult = SetParametersResult
50+ QosCallbackResult : 'TypeAlias' = SetParametersResult
4551# Qos callback type annotation
4652QosCallbackType = Callable [[QoSProfile ], QosCallbackResult ]
4753
@@ -103,7 +109,7 @@ def _declare_qos_parameters(
103109 topic_name : Text ,
104110 qos : QoSProfile ,
105111 options : QoSOverridingOptions
106- ) -> QoSProfile :
112+ ) -> None :
107113 """
108114 Declare QoS parameters for a Publisher or a Subscription.
109115
@@ -143,37 +149,42 @@ def _declare_qos_parameters(
143149 f"{ description .format ('Provided QoS overrides' )} , are not valid: { result .reason } " )
144150
145151
146- def _get_allowed_policies (entity_type : Union [Type [Publisher ], Type [Subscription ]]):
152+ def _get_allowed_policies (entity_type : Union [Type [Publisher ],
153+ Type [Subscription ]]) -> List [QoSPolicyKind ]:
147154 allowed_policies = list (QoSPolicyKind .__members__ .values ())
148155 if issubclass (entity_type , Subscription ):
149156 allowed_policies .remove (QoSPolicyKind .LIFESPAN )
150157 return allowed_policies
151158
152159
160+ QoSProfileAttributes = Union [QoSHistoryPolicy , int , QoSReliabilityPolicy , QoSDurabilityPolicy ,
161+ Duration , QoSLivelinessPolicy , bool ]
162+
163+
153164def _get_qos_policy_parameter (qos : QoSProfile , policy : QoSPolicyKind ) -> Union [str , int , bool ]:
154- value = getattr (qos , policy .name .lower ())
155- if policy in (
156- QoSPolicyKind .LIVELINESS , QoSPolicyKind .RELIABILITY ,
157- QoSPolicyKind .HISTORY , QoSPolicyKind .DURABILITY
158- ):
159- value = value .name .lower ()
160- if value == 'unknown' :
165+ value : QoSProfileAttributes = getattr (qos , policy .name .lower ())
166+ if isinstance (value , (QoSHistoryPolicy , QoSReliabilityPolicy ,
167+ QoSDurabilityPolicy , QoSLivelinessPolicy )):
168+ return_value : Union [str , int , bool ] = value .name .lower ()
169+ if return_value == 'unknown' :
161170 raise ValueError ('User provided QoS profile is invalid' )
162- if policy in (
163- QoSPolicyKind . LIFESPAN , QoSPolicyKind . DEADLINE , QoSPolicyKind . LIVELINESS_LEASE_DURATION
164- ) :
165- value = value . nanoseconds
166- return value
171+ elif isinstance ( value , Duration ):
172+ return_value = value . nanoseconds
173+ else :
174+ return_value = value
175+ return return_value
167176
168177
169- def _override_qos_policy_with_param (qos : QoSProfile , policy : QoSPolicyKind , param : Parameter ):
178+ def _override_qos_policy_with_param (qos : QoSProfile ,
179+ policy : QoSPolicyKind ,
180+ param : Parameter ) -> None :
170181 value = param .value
171182 policy_name = policy .name .lower ()
172183 if policy in (
173184 QoSPolicyKind .LIVELINESS , QoSPolicyKind .RELIABILITY ,
174185 QoSPolicyKind .HISTORY , QoSPolicyKind .DURABILITY
175186 ):
176- def capitalize_first_letter (x ) :
187+ def capitalize_first_letter (x : str ) -> str :
177188 return x [0 ].upper () + x [1 :]
178189 # e.g. `policy=QosPolicyKind.LIVELINESS` -> `policy_enum_class=rclpy.qos.LivelinessPolicy`
179190 policy_enum_class = getattr (
0 commit comments