From 51d410dc6816532f0fe1266a8ef0b1da8f8d9370 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Sat, 9 Mar 2024 19:05:32 -0500 Subject: [PATCH 01/23] Add types to parameter.py Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 56 ++++++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 25 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 7dce99b8b..7ffdef046 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -13,12 +13,13 @@ # limitations under the License. import array -from enum import Enum +from enum import IntEnum from typing import Dict +from typing import Generic from typing import List from typing import Optional from typing import Tuple -from typing import TYPE_CHECKING +from typing import TypeVar from typing import Union from rcl_interfaces.msg import Parameter as ParameterMsg @@ -28,18 +29,19 @@ PARAMETER_SEPARATOR_STRING = '.' -if TYPE_CHECKING: - AllowableParameterValue = Union[None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], - List[bool], Tuple[bool, ...], - List[int], Tuple[int, ...], array.array[int], - List[float], Tuple[float, ...], array.array[float], - List[str], Tuple[str, ...], array.array[str]] +AllowableParameterValue = Union[None, bool, int, float, str, + List[bytes], Tuple[bytes, ...], + List[bool], Tuple[bool, ...], + List[int], Tuple[int, ...], 'array.array[int]', + List[float], Tuple[float, ...], 'array.array[float]', + List[str], Tuple[str, ...], 'array.array[str]'] +AllowableParameterValueT = TypeVar('AllowableParameterValueT', AllowableParameterValue, None) -class Parameter: - class Type(Enum): +class Parameter(Generic[AllowableParameterValueT]): + + class Type(IntEnum): NOT_SET = ParameterType.PARAMETER_NOT_SET BOOL = ParameterType.PARAMETER_BOOL INTEGER = ParameterType.PARAMETER_INTEGER @@ -52,7 +54,8 @@ class Type(Enum): STRING_ARRAY = ParameterType.PARAMETER_STRING_ARRAY @classmethod - def from_parameter_value(cls, parameter_value): + def from_parameter_value(cls, + parameter_value: AllowableParameterValue) -> 'Parameter.Type': """ Get a Parameter.Type from a given variable. @@ -88,7 +91,7 @@ def from_parameter_value(cls, parameter_value): raise TypeError( f"The given value is not one of the allowed types '{parameter_value}'.") - def check(self, parameter_value): + def check(self, parameter_value: AllowableParameterValue) -> bool: if Parameter.Type.NOT_SET == self: return parameter_value is None if Parameter.Type.BOOL == self: @@ -117,7 +120,7 @@ def check(self, parameter_value): return False @classmethod - def from_parameter_msg(cls, param_msg): + def from_parameter_msg(cls, param_msg: ParameterMsg) -> 'Parameter[AllowableParameterValueT]': value = None type_ = Parameter.Type(value=param_msg.value.type) if Parameter.Type.BOOL == type_: @@ -140,7 +143,9 @@ def from_parameter_msg(cls, param_msg): value = param_msg.value.string_array_value return cls(param_msg.name, type_, value) - def __init__(self, name, type_=None, value=None): + def __init__(self, name: str, + type_: Optional['Parameter.Type'] = None, + value: AllowableParameterValueT = None) -> None: if type_ is None: # This will raise a TypeError if it is not possible to get a type from the value. type_ = Parameter.Type.from_parameter_value(value) @@ -156,18 +161,18 @@ def __init__(self, name, type_=None, value=None): self._value = value @property - def name(self): + def name(self) -> str: return self._name @property - def type_(self): + def type_(self) -> 'Parameter.Type': return self._type_ @property - def value(self): + def value(self) -> AllowableParameterValueT: return self._value - def get_parameter_value(self): + def get_parameter_value(self) -> ParameterValue: parameter_value = ParameterValue(type=self.type_.value) if Parameter.Type.BOOL == self.type_: parameter_value.bool_value = self.value @@ -189,7 +194,7 @@ def get_parameter_value(self): parameter_value.string_array_value = self.value return parameter_value - def to_parameter_msg(self): + def to_parameter_msg(self) -> ParameterMsg: return ParameterMsg(name=self.name, value=self.get_parameter_value()) @@ -237,7 +242,7 @@ def get_parameter_value(string_value: str) -> ParameterValue: return value -def parameter_value_to_python(parameter_value: ParameterValue): +def parameter_value_to_python(parameter_value: ParameterValue) -> AllowableParameterValue: """ Get the value for the Python builtin type from a rcl_interfaces/msg/ParameterValue object. @@ -295,7 +300,7 @@ def parameter_dict_from_yaml_file( """ with open(parameter_file, 'r') as f: param_file = yaml.safe_load(f) - param_keys = [] + param_keys: List[str] = [] param_dict = {} if use_wildcard and '/**' in param_file: @@ -319,13 +324,14 @@ def parameter_dict_from_yaml_file( for n in param_keys: value = param_file[n] - if type(value) != dict or 'ros__parameters' not in value: + if not isinstance(value, dict) or 'ros__parameters' not in value: raise RuntimeError(f'YAML file is not a valid ROS parameter file for node {n}') param_dict.update(value['ros__parameters']) return _unpack_parameter_dict(namespace, param_dict) -def _unpack_parameter_dict(namespace, parameter_dict): +def _unpack_parameter_dict(namespace: str, + parameter_dict: Dict[str, ParameterMsg]) -> Dict[str, ParameterMsg]: """ Flatten a parameter dictionary recursively. @@ -337,7 +343,7 @@ def _unpack_parameter_dict(namespace, parameter_dict): for param_name, param_value in parameter_dict.items(): full_param_name = namespace + param_name # Unroll nested parameters - if type(param_value) == dict: + if isinstance(param_value, dict): parameters.update(_unpack_parameter_dict( namespace=full_param_name + PARAMETER_SEPARATOR_STRING, parameter_dict=param_value)) From 7661294ca5855bfa68619534795f769ee96f6afe Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 11 Mar 2024 08:43:18 -0400 Subject: [PATCH 02/23] Revert "Add types to TypeHash and moved away from __slots__ usage (#1232)" (#1243) This reverts commit b06baefa0b0cebd293b1dec90627641257de1db3. Signed-off-by: Michael Carlstrom --- rclpy/rclpy/type_hash.py | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/rclpy/rclpy/type_hash.py b/rclpy/rclpy/type_hash.py index 30b4e0b8e..f2f3ea4db 100644 --- a/rclpy/rclpy/type_hash.py +++ b/rclpy/rclpy/type_hash.py @@ -18,44 +18,56 @@ class TypeHash: _TYPE_HASH_SIZE = 32 - def __init__(self, version: int = -1, value: bytes = bytes(_TYPE_HASH_SIZE)): - self.version = version - self.value = value + __slots__ = [ + '_version', + '_value', + ] + + def __init__(self, **kwargs): + assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ + 'Invalid arguments passed to constructor: %r' % kwargs.keys() + + self.version = kwargs.get('version', -1) + self.value = kwargs.get('value', bytes(self._TYPE_HASH_SIZE)) @property - def version(self) -> int: + def version(self): """ Get field 'version'. :returns: version attribute + :rtype: int """ return self._version @version.setter - def version(self, value: int) -> None: + def version(self, value): assert isinstance(value, int) self._version = value @property - def value(self) -> bytes: + def value(self): """ Get field 'value'. :returns: value attribute + :rtype: bytes """ return self._value @value.setter - def value(self, value: bytes) -> None: + def value(self, value): assert isinstance(value, bytes) self._value = value - def __eq__(self, other: object) -> bool: + def __eq__(self, other): if not isinstance(other, TypeHash): return False - return self.__dict__ == other.__dict__ + return all( + self.__getattribute__(slot) == other.__getattribute__(slot) + for slot in self.__slots__) - def __str__(self) -> str: + def __str__(self): if self._version <= 0 or len(self._value) != self._TYPE_HASH_SIZE: return 'INVALID' From 98ba24cc61a8dad66f553eaccfe055cff4a507a1 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom <36806982+InvincibleRMC@users.noreply.github.com> Date: Tue, 12 Mar 2024 06:56:16 -0700 Subject: [PATCH 03/23] Add back Type hash __slots__ and add test cases. (#1245) * Add types to TypeHash and add test cases Signed-off-by: Michael Carlstrom --- rclpy/rclpy/type_hash.py | 25 +++++++++++-------------- rclpy/test/test_type_hash.py | 5 +++++ 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/rclpy/rclpy/type_hash.py b/rclpy/rclpy/type_hash.py index f2f3ea4db..ae7daf12f 100644 --- a/rclpy/rclpy/type_hash.py +++ b/rclpy/rclpy/type_hash.py @@ -18,56 +18,53 @@ class TypeHash: _TYPE_HASH_SIZE = 32 + # ros2cli needs __slots__ to avoid API break from https://github.com/ros2/rclpy/pull/1243. + # __slots__ is also used improve performance of Python objects. __slots__ = [ '_version', '_value', ] - def __init__(self, **kwargs): - assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ - 'Invalid arguments passed to constructor: %r' % kwargs.keys() - - self.version = kwargs.get('version', -1) - self.value = kwargs.get('value', bytes(self._TYPE_HASH_SIZE)) + def __init__(self, version: int = -1, value: bytes = bytes(_TYPE_HASH_SIZE)): + self.version = version + self.value = value @property - def version(self): + def version(self) -> int: """ Get field 'version'. :returns: version attribute - :rtype: int """ return self._version @version.setter - def version(self, value): + def version(self, value: int) -> None: assert isinstance(value, int) self._version = value @property - def value(self): + def value(self) -> bytes: """ Get field 'value'. :returns: value attribute - :rtype: bytes """ return self._value @value.setter - def value(self, value): + def value(self, value: bytes) -> None: assert isinstance(value, bytes) self._value = value - def __eq__(self, other): + def __eq__(self, other: object) -> bool: if not isinstance(other, TypeHash): return False return all( self.__getattribute__(slot) == other.__getattribute__(slot) for slot in self.__slots__) - def __str__(self): + def __str__(self) -> str: if self._version <= 0 or len(self._value) != self._TYPE_HASH_SIZE: return 'INVALID' diff --git a/rclpy/test/test_type_hash.py b/rclpy/test/test_type_hash.py index 390330725..91318b706 100644 --- a/rclpy/test/test_type_hash.py +++ b/rclpy/test/test_type_hash.py @@ -30,6 +30,7 @@ class TestTypeHash(unittest.TestCase): def test_dict_constructor(self): type_hash = TypeHash(**STD_MSGS_STRING_TYPE_HASH_DICT) + self.assertTrue(hasattr(type_hash, '__slots__')) self.assertEqual(STD_MSGS_STRING_TYPE_HASH_DICT['version'], type_hash.version) self.assertEqual(STD_MSGS_STRING_TYPE_HASH_DICT['value'], type_hash.value) @@ -42,3 +43,7 @@ def test_print_invalid(self): actual_str = str(TypeHash()) expected_str = 'INVALID' self.assertEqual(expected_str, actual_str) + + def test_equals(self): + self.assertEqual(TypeHash(), TypeHash()) + self.assertNotEqual(TypeHash(version=5), TypeHash()) From c60ba6fcdcc5007c2baa6d931f2645483b03c690 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom <36806982+InvincibleRMC@users.noreply.github.com> Date: Thu, 14 Mar 2024 16:20:33 -0700 Subject: [PATCH 04/23] Add types to context.py (#1240) Signed-off-by: Michael Carlstrom --- rclpy/rclpy/context.py | 66 ++++++++++++++++++++++++++------------ rclpy/rclpy/destroyable.py | 29 +++++++++++++++++ 2 files changed, 75 insertions(+), 20 deletions(-) create mode 100644 rclpy/rclpy/destroyable.py diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index 429a442ce..c3a27953e 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -15,17 +15,33 @@ from inspect import ismethod import sys import threading -from types import TracebackType +from types import MethodType, TracebackType from typing import Callable from typing import ContextManager from typing import List from typing import Optional +from typing import Protocol from typing import Type -import weakref +from typing import Union +from weakref import WeakMethod + +from rclpy.destroyable import DestroyableType + + +class ContextHandle(DestroyableType, Protocol): + + def ok(self) -> bool: + ... + + def get_domain_id(self) -> int: + ... + + def shutdown(self) -> None: + ... g_logging_configure_lock = threading.Lock() -g_logging_ref_count = 0 +g_logging_ref_count: int = 0 class Context(ContextManager['Context']): @@ -43,24 +59,25 @@ class Context(ContextManager['Context']): """ - def __init__(self): + def __init__(self) -> None: self._lock = threading.Lock() - self._callbacks = [] + self._callbacks: List[Union['WeakMethod[MethodType]', Callable[[], None]]] = [] self._logging_initialized = False - self.__context = None + self.__context: Optional[ContextHandle] = None @property - def handle(self): + def handle(self) -> Optional[ContextHandle]: return self.__context - def destroy(self): - self.__context.destroy_when_not_in_use() + def destroy(self) -> None: + if self.__context: + self.__context.destroy_when_not_in_use() def init(self, args: Optional[List[str]] = None, *, initialize_logging: bool = True, - domain_id: Optional[int] = None): + domain_id: Optional[int] = None) -> None: """ Initialize ROS communications for a given context. @@ -89,7 +106,7 @@ def init(self, _rclpy.rclpy_logging_configure(self.__context) self._logging_initialized = True - def ok(self): + def ok(self) -> bool: """Check if context hasn't been shut down.""" with self._lock: if self.__context is None: @@ -97,14 +114,14 @@ def ok(self): with self.__context: return self.__context.ok() - def _call_on_shutdown_callbacks(self): + def _call_on_shutdown_callbacks(self) -> None: for weak_method in self._callbacks: callback = weak_method() if callback is not None: callback() self._callbacks = [] - def shutdown(self): + def shutdown(self) -> None: """Shutdown this context.""" if self.__context is None: raise RuntimeError('Context must be initialized before it can be shutdown') @@ -113,7 +130,7 @@ def shutdown(self): self._call_on_shutdown_callbacks() self._logging_fini() - def try_shutdown(self): + def try_shutdown(self) -> None: """Shutdown this context, if not already shutdown.""" if self.__context is None: return @@ -123,23 +140,32 @@ def try_shutdown(self): self._call_on_shutdown_callbacks() self._logging_fini() - def _remove_callback(self, weak_method): + def _remove_callback(self, weak_method: 'WeakMethod[MethodType]') -> None: self._callbacks.remove(weak_method) - def on_shutdown(self, callback: Callable[[], None]): + def on_shutdown(self, callback: Callable[[], None]) -> None: """Add a callback to be called on shutdown.""" if not callable(callback): raise TypeError('callback should be a callable, got {}', type(callback)) + + if self.__context is None: + with self._lock: + if ismethod(callback): + self._callbacks.append(WeakMethod(callback, self._remove_callback)) + else: + self._callbacks.append(callback) + return + with self.__context, self._lock: if not self.__context.ok(): callback() else: if ismethod(callback): - self._callbacks.append(weakref.WeakMethod(callback, self._remove_callback)) + self._callbacks.append(WeakMethod(callback, self._remove_callback)) else: self._callbacks.append(callback) - def _logging_fini(self): + def _logging_fini(self) -> None: # This function must be called with self._lock held. from rclpy.impl.implementation_singleton import rclpy_implementation global g_logging_ref_count @@ -153,7 +179,7 @@ def _logging_fini(self): 'Unexpected error: logger ref count should never be lower that zero') self._logging_initialized = False - def get_domain_id(self): + def get_domain_id(self) -> int: """Get domain id of context.""" if self.__context is None: raise RuntimeError('Context must be initialized before it can have a domain id') @@ -162,7 +188,7 @@ def get_domain_id(self): def __enter__(self) -> 'Context': # We do not accept parameters here. If one wants to customize the init() call, - # they would have to call it manaully and not use the ContextManager convenience + # they would have to call it manually and not use the ContextManager convenience self.init() return self diff --git a/rclpy/rclpy/destroyable.py b/rclpy/rclpy/destroyable.py new file mode 100644 index 000000000..8c057923d --- /dev/null +++ b/rclpy/rclpy/destroyable.py @@ -0,0 +1,29 @@ +# Copyright 2024 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. + +from types import TracebackType +from typing import Optional, Protocol, Type + + +class DestroyableType(Protocol): + + def __enter__(self) -> None: + ... + + def __exit__(self, exc_type: Optional[Type[BaseException]], + exc_val: Optional[BaseException], exctb: Optional[TracebackType]) -> None: + ... + + def destroy_when_not_in_use(self) -> None: + ... From 0bca600e77e21d1bfa8900ae3fbef8c79d315b48 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom <36806982+InvincibleRMC@users.noreply.github.com> Date: Sun, 24 Mar 2024 19:57:16 -0400 Subject: [PATCH 05/23] Add types to qos_overriding_options.py (#1248) Signed-off-by: Michael Carlstrom Signed-off-by: Michael Carlstrom --- rclpy/rclpy/qos_overriding_options.py | 45 +++++++++++++++++---------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/rclpy/rclpy/qos_overriding_options.py b/rclpy/rclpy/qos_overriding_options.py index 316b59433..f64b83d30 100644 --- a/rclpy/rclpy/qos_overriding_options.py +++ b/rclpy/rclpy/qos_overriding_options.py @@ -14,6 +14,7 @@ from typing import Callable from typing import Iterable +from typing import List from typing import Optional from typing import Text from typing import Type @@ -28,11 +29,16 @@ from rclpy.exceptions import ParameterAlreadyDeclaredException from rclpy.parameter import Parameter from rclpy.publisher import Publisher +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSLivelinessPolicy from rclpy.qos import QoSPolicyKind from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy from rclpy.subscription import Subscription if TYPE_CHECKING: + from typing import TypeAlias from rclpy.node import Node @@ -41,7 +47,7 @@ class InvalidQosOverridesError(Exception): # Return type of qos validation callbacks -QosCallbackResult = SetParametersResult +QosCallbackResult: 'TypeAlias' = SetParametersResult # Qos callback type annotation QosCallbackType = Callable[[QoSProfile], QosCallbackResult] @@ -103,7 +109,7 @@ def _declare_qos_parameters( topic_name: Text, qos: QoSProfile, options: QoSOverridingOptions -) -> QoSProfile: +) -> None: """ Declare QoS parameters for a Publisher or a Subscription. @@ -143,37 +149,42 @@ def _declare_qos_parameters( f"{description.format('Provided QoS overrides')}, are not valid: {result.reason}") -def _get_allowed_policies(entity_type: Union[Type[Publisher], Type[Subscription]]): +def _get_allowed_policies(entity_type: Union[Type[Publisher], + Type[Subscription]]) -> List[QoSPolicyKind]: allowed_policies = list(QoSPolicyKind.__members__.values()) if issubclass(entity_type, Subscription): allowed_policies.remove(QoSPolicyKind.LIFESPAN) return allowed_policies +QoSProfileAttributes = Union[QoSHistoryPolicy, int, QoSReliabilityPolicy, QoSDurabilityPolicy, + Duration, QoSLivelinessPolicy, bool] + + def _get_qos_policy_parameter(qos: QoSProfile, policy: QoSPolicyKind) -> Union[str, int, bool]: - value = getattr(qos, policy.name.lower()) - if policy in ( - QoSPolicyKind.LIVELINESS, QoSPolicyKind.RELIABILITY, - QoSPolicyKind.HISTORY, QoSPolicyKind.DURABILITY - ): - value = value.name.lower() - if value == 'unknown': + value: QoSProfileAttributes = getattr(qos, policy.name.lower()) + if isinstance(value, (QoSHistoryPolicy, QoSReliabilityPolicy, + QoSDurabilityPolicy, QoSLivelinessPolicy)): + return_value: Union[str, int, bool] = value.name.lower() + if return_value == 'unknown': raise ValueError('User provided QoS profile is invalid') - if policy in ( - QoSPolicyKind.LIFESPAN, QoSPolicyKind.DEADLINE, QoSPolicyKind.LIVELINESS_LEASE_DURATION - ): - value = value.nanoseconds - return value + elif isinstance(value, Duration): + return_value = value.nanoseconds + else: + return_value = value + return return_value -def _override_qos_policy_with_param(qos: QoSProfile, policy: QoSPolicyKind, param: Parameter): +def _override_qos_policy_with_param(qos: QoSProfile, + policy: QoSPolicyKind, + param: Parameter) -> None: value = param.value policy_name = policy.name.lower() if policy in ( QoSPolicyKind.LIVELINESS, QoSPolicyKind.RELIABILITY, QoSPolicyKind.HISTORY, QoSPolicyKind.DURABILITY ): - def capitalize_first_letter(x): + def capitalize_first_letter(x: str) -> str: return x[0].upper() + x[1:] # e.g. `policy=QosPolicyKind.LIVELINESS` -> `policy_enum_class=rclpy.qos.LivelinessPolicy` policy_enum_class = getattr( From 51889a4576b1fcd2e3a392b73d09c7b714d04032 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 26 Mar 2024 14:31:37 -0400 Subject: [PATCH 06/23] Small fixes for modern flake8. (#1264) It doesn't like to compare types with ==, so switch to isinstance as appropriate. Signed-off-by: Chris Lalancette Signed-off-by: Michael Carlstrom --- rclpy/test/test_logging_rosout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/test/test_logging_rosout.py b/rclpy/test/test_logging_rosout.py index 1de1c73a6..1e170a317 100644 --- a/rclpy/test/test_logging_rosout.py +++ b/rclpy/test/test_logging_rosout.py @@ -83,7 +83,7 @@ def test_enable_rosout( if expected_data: assert (rosout_subscription_msg is not None) - assert (type(rosout_subscription_msg) == Log) + assert (isinstance(rosout_subscription_msg, Log)) assert (LoggingSeverity(rosout_subscription_msg.level) == LoggingSeverity.INFO) assert (len(rosout_subscription_msg.msg) != 0) assert (rosout_subscription_msg.msg == message_data) From 6b5b7938206d82d025caf8f8836e6259b8ab40d4 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom <36806982+InvincibleRMC@users.noreply.github.com> Date: Wed, 27 Mar 2024 08:07:21 -0400 Subject: [PATCH 07/23] Add types to time_source.py (#1259) * Add types to time_source.py Signed-off-by: Michael Carlstrom --- rclpy/rclpy/time_source.py | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/rclpy/rclpy/time_source.py b/rclpy/rclpy/time_source.py index 8975397eb..aa0e2a10e 100644 --- a/rclpy/rclpy/time_source.py +++ b/rclpy/rclpy/time_source.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import List, Optional, Set, TYPE_CHECKING import weakref from rcl_interfaces.msg import SetParametersResult @@ -23,16 +24,20 @@ from rclpy.time import Time import rosgraph_msgs.msg +if TYPE_CHECKING: + from rclpy.node import Node + from rclpy.subscription import Subscription + CLOCK_TOPIC = '/clock' USE_SIM_TIME_NAME = 'use_sim_time' class TimeSource: - def __init__(self, *, node=None): - self._clock_sub = None - self._node_weak_ref = None - self._associated_clocks = set() + def __init__(self, *, node: Optional['Node'] = None): + self._clock_sub: Optional['Subscription'] = None + self._node_weak_ref: Optional[weakref.ReferenceType['Node']] = None + self._associated_clocks: Set[ROSClock] = set() # Zero time is a special value that means time is uninitialzied self._last_time_set = Time(clock_type=ClockType.ROS_TIME) self._ros_time_is_active = False @@ -40,11 +45,11 @@ def __init__(self, *, node=None): self.attach_node(node) @property - def ros_time_is_active(self): + def ros_time_is_active(self) -> bool: return self._ros_time_is_active @ros_time_is_active.setter - def ros_time_is_active(self, enabled): + def ros_time_is_active(self, enabled: bool) -> None: if self._ros_time_is_active == enabled: return self._ros_time_is_active = enabled @@ -59,7 +64,7 @@ def ros_time_is_active(self, enabled): node.destroy_subscription(self._clock_sub) self._clock_sub = None - def _subscribe_to_clock_topic(self): + def _subscribe_to_clock_topic(self) -> None: if self._clock_sub is None: node = self._get_node() if node is not None: @@ -70,7 +75,7 @@ def _subscribe_to_clock_topic(self): QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT) ) - def attach_node(self, node): + def attach_node(self, node: 'Node') -> None: from rclpy.node import Node if not isinstance(node, Node): raise TypeError('Node must be of type rclpy.node.Node') @@ -97,7 +102,7 @@ def attach_node(self, node): node.add_on_set_parameters_callback(self._on_parameter_event) - def detach_node(self): + def detach_node(self) -> None: # Remove the subscription to the clock topic. if self._clock_sub is not None: node = self._get_node() @@ -107,7 +112,7 @@ def detach_node(self): self._clock_sub = None self._node_weak_ref = None - def attach_clock(self, clock): + def attach_clock(self, clock: ROSClock) -> None: if not isinstance(clock, ROSClock): raise ValueError('Only clocks with type ROS_TIME can be attached.') @@ -115,14 +120,14 @@ def attach_clock(self, clock): clock._set_ros_time_is_active(self.ros_time_is_active) self._associated_clocks.add(clock) - def clock_callback(self, msg): + def clock_callback(self, msg: rosgraph_msgs.msg.Clock) -> None: # Cache the last message in case a new clock is attached. time_from_msg = Time.from_msg(msg.clock) self._last_time_set = time_from_msg for clock in self._associated_clocks: clock.set_ros_time_override(time_from_msg) - def _on_parameter_event(self, parameter_list): + def _on_parameter_event(self, parameter_list: List[Parameter]) -> SetParametersResult: successful = True reason = '' @@ -142,7 +147,7 @@ def _on_parameter_event(self, parameter_list): return SetParametersResult(successful=successful, reason=reason) - def _get_node(self): + def _get_node(self) -> Optional['Node']: if self._node_weak_ref is not None: return self._node_weak_ref() return None From fe29332abe03451cd9f26d21eb226c041b445d74 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 27 Mar 2024 14:23:55 -0400 Subject: [PATCH 08/23] fix small bug Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 7ffdef046..b276e14a3 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -36,7 +36,12 @@ List[float], Tuple[float, ...], 'array.array[float]', List[str], Tuple[str, ...], 'array.array[str]'] -AllowableParameterValueT = TypeVar('AllowableParameterValueT', AllowableParameterValue, None) + +AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, + List[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], + List[int], Tuple[int, ...], 'array.array[int]', + List[float], Tuple[float, ...], 'array.array[float]', + List[str], Tuple[str, ...], 'array.array[str]') class Parameter(Generic[AllowableParameterValueT]): @@ -55,7 +60,8 @@ class Type(IntEnum): @classmethod def from_parameter_value(cls, - parameter_value: AllowableParameterValue) -> 'Parameter.Type': + parameter_value: AllowableParameterValueT + ) -> 'Parameter.Type': """ Get a Parameter.Type from a given variable. @@ -91,7 +97,7 @@ def from_parameter_value(cls, raise TypeError( f"The given value is not one of the allowed types '{parameter_value}'.") - def check(self, parameter_value: AllowableParameterValue) -> bool: + def check(self, parameter_value: AllowableParameterValueT) -> bool: if Parameter.Type.NOT_SET == self: return parameter_value is None if Parameter.Type.BOOL == self: @@ -143,9 +149,11 @@ def from_parameter_msg(cls, param_msg: ParameterMsg) -> 'Parameter[AllowablePara value = param_msg.value.string_array_value return cls(param_msg.name, type_, value) + # value: AllowableParameterValueT = None Needs a type: ignore due to TypeVars not inferring + # types of defaults yet https://github.com/python/mypy/issues/3737# def __init__(self, name: str, type_: Optional['Parameter.Type'] = None, - value: AllowableParameterValueT = None) -> None: + value: AllowableParameterValueT = None) -> None: # type: ignore[assignment] if type_ is None: # This will raise a TypeError if it is not possible to get a type from the value. type_ = Parameter.Type.from_parameter_value(value) @@ -158,7 +166,7 @@ def __init__(self, name: str, self._type_ = type_ self._name = name - self._value = value + self._value: AllowableParameterValueT = value @property def name(self) -> str: From 2655545f7efb9328a5394ac7dd2a61bb50274e1a Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 15:04:24 -0400 Subject: [PATCH 09/23] Switch to overloads to avoid mypy3737 Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index b276e14a3..cf1453765 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -18,6 +18,7 @@ from typing import Generic from typing import List from typing import Optional +from typing import overload from typing import Tuple from typing import TypeVar from typing import Union @@ -149,11 +150,14 @@ def from_parameter_msg(cls, param_msg: ParameterMsg) -> 'Parameter[AllowablePara value = param_msg.value.string_array_value return cls(param_msg.name, type_, value) - # value: AllowableParameterValueT = None Needs a type: ignore due to TypeVars not inferring - # types of defaults yet https://github.com/python/mypy/issues/3737# - def __init__(self, name: str, - type_: Optional['Parameter.Type'] = None, - value: AllowableParameterValueT = None) -> None: # type: ignore[assignment] + @overload + def __init__(self, name: str, type_: Optional['Parameter.Type'] = None) -> None: ... + + @overload + def __init__(self, name: str, type_: Optional['Parameter.Type'], + value: AllowableParameterValueT) -> None: ... + + def __init__(self, name, type_=None, value=None): if type_ is None: # This will raise a TypeError if it is not possible to get a type from the value. type_ = Parameter.Type.from_parameter_value(value) @@ -166,7 +170,7 @@ def __init__(self, name: str, self._type_ = type_ self._name = name - self._value: AllowableParameterValueT = value + self._value = value @property def name(self) -> str: From dc58c0b3e1b816d0a00d6411d6c0406c96b983ad Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 15:40:14 -0400 Subject: [PATCH 10/23] Update parameter declaration from Node Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index e21f523b6..dfe8413a7 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -15,11 +15,11 @@ import math import time -from typing import Any from typing import Callable from typing import Dict from typing import Iterator from typing import List +from typing import overload from typing import Optional from typing import Sequence from typing import Tuple @@ -66,7 +66,7 @@ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger from rclpy.logging_service import LoggingService -from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING +from parameter import Parameter, PARAMETER_SEPARATOR_STRING, AllowableParameterValueT, AllowableParameterValue from rclpy.parameter_service import ParameterService from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events @@ -355,10 +355,21 @@ def get_logger(self): """Get the nodes logger.""" return self._logger + @overload + def declare_parameter(self, name: str, value: None = None, + descriptor: Optional[ParameterDescriptor] = None, + ignore_override: bool = False) -> Parameter[None]: ... + + @overload + def declare_parameter(self, name: str, value: AllowableParameterValueT, + descriptor: Optional[ParameterDescriptor] = None, + ignore_override: bool = False + ) -> Parameter[AllowableParameterValueT]: ... + def declare_parameter( self, name: str, - value: Any = None, + value: AllowableParameterValue = None, descriptor: Optional[ParameterDescriptor] = None, ignore_override: bool = False ) -> Parameter: @@ -383,7 +394,8 @@ def declare_parameter( """ if value is None and descriptor is None: # Temporal patch so we get deprecation warning if only a name is provided. - args = (name, ) + args: Union[Tuple[str], Tuple[str, AllowableParameterValue, + ParameterDescriptor]] = (name, ) else: descriptor = ParameterDescriptor() if descriptor is None else descriptor args = (name, value, descriptor) @@ -395,7 +407,7 @@ def declare_parameters( parameters: List[Union[ Tuple[str], Tuple[str, Parameter.Type], - Tuple[str, Any, ParameterDescriptor], + Tuple[str, AllowableParameterValue, ParameterDescriptor], ]], ignore_override: bool = False ) -> List[Parameter]: From da1251b3b3e9843f278fb27b3eccb94eaff369a5 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 15:42:25 -0400 Subject: [PATCH 11/23] add back rclpy. Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index dfe8413a7..6f834f123 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -66,7 +66,7 @@ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger from rclpy.logging_service import LoggingService -from parameter import Parameter, PARAMETER_SEPARATOR_STRING, AllowableParameterValueT, AllowableParameterValue +from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING, AllowableParameterValueT, AllowableParameterValue from rclpy.parameter_service import ParameterService from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events From f7fa348899f028c10ac581ba7ac1f40dbd6e0f91 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 15:51:02 -0400 Subject: [PATCH 12/23] Fix flake8 imports Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 6f834f123..bebbf041d 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -19,8 +19,8 @@ from typing import Dict from typing import Iterator from typing import List -from typing import overload from typing import Optional +from typing import overload from typing import Sequence from typing import Tuple from typing import Type @@ -66,7 +66,8 @@ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger from rclpy.logging_service import LoggingService -from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING, AllowableParameterValueT, AllowableParameterValue +from rclpy.parameter import (AllowableParameterValue, AllowableParameterValueT, Parameter, + PARAMETER_SEPARATOR_STRING) from rclpy.parameter_service import ParameterService from rclpy.publisher import Publisher from rclpy.qos import qos_profile_parameter_events From d60df586ece58c89ededeb5fe0f12daea43fe014 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 16:59:23 -0400 Subject: [PATCH 13/23] Add proper array.array[] Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 6 +++--- rclpy/rclpy/parameter.py | 41 +++++++++++++++++++++++++++------------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index bebbf041d..a460833b3 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -179,7 +179,7 @@ def __init__( self._post_set_parameters_callbacks: List[Callable[[List[Parameter]], None]] = [] self._rate_group = ReentrantCallbackGroup() self._allow_undeclared_parameters = allow_undeclared_parameters - self._parameter_overrides = {} + self._parameter_overrides: Dict[str, Parameter] = {} self._descriptors = {} namespace = namespace or '' @@ -458,8 +458,8 @@ def declare_parameters( :raises: InvalidParameterValueException if the registered callback rejects any parameter. :raises: TypeError if any tuple in **parameters** does not match the annotated type. """ - parameter_list = [] - descriptors = {} + parameter_list: List[Parameter] = [] + descriptors: Dict[str, ParameterDescriptor] = {} for index, parameter_tuple in enumerate(parameters): if len(parameter_tuple) < 1 or len(parameter_tuple) > 3: raise TypeError( diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index cf1453765..ae4ca23c2 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import sys import array from enum import IntEnum from typing import Dict @@ -30,19 +31,33 @@ PARAMETER_SEPARATOR_STRING = '.' -AllowableParameterValue = Union[None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], - List[bool], Tuple[bool, ...], - List[int], Tuple[int, ...], 'array.array[int]', - List[float], Tuple[float, ...], 'array.array[float]', - List[str], Tuple[str, ...], 'array.array[str]'] - - -AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], - List[int], Tuple[int, ...], 'array.array[int]', - List[float], Tuple[float, ...], 'array.array[float]', - List[str], Tuple[str, ...], 'array.array[str]') +# Mypy does not handle string literals of array.array[int/str/float] very well +# So if user has newer version of python can use proper arrays. +if sys.version_info > (3, 9): + AllowableParameterValue = Union[None, bool, int, float, str, + list[bytes], Tuple[bytes, ...], + list[bool], Tuple[bool, ...], + list[int], Tuple[int, ...], array.array[int], + list[float], Tuple[float, ...], array.array[float], + list[str], Tuple[str, ...], array.array[str]] + AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, + list[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], + list[int], Tuple[int, ...], array.array[int], + list[float], Tuple[float, ...], array.array[float], + list[str], Tuple[str, ...], array.array[str]) +else: + AllowableParameterValue = Union[None, bool, int, float, str, + List[bytes], Tuple[bytes, ...], + List[bool], Tuple[bool, ...], + List[int], Tuple[int, ...], 'array.array[int]', + List[float], Tuple[float, ...], 'array.array[float]', + List[str], Tuple[str, ...], 'array.array[str]'] + + AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, + List[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], + List[int], Tuple[int, ...], 'array.array[int]', + List[float], Tuple[float, ...], 'array.array[float]', + List[str], Tuple[str, ...], 'array.array[str]') class Parameter(Generic[AllowableParameterValueT]): From 44f4fba625bef481e45f639bf732206daf652832 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 21:10:51 -0400 Subject: [PATCH 14/23] Update types of declare_parameter, declare_parameters api Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 39 +++++++++++++++++++++------------------ rclpy/rclpy/parameter.py | 14 ++++++++------ 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index a460833b3..bd43f75a4 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -21,7 +21,6 @@ from typing import List from typing import Optional from typing import overload -from typing import Sequence from typing import Tuple from typing import Type from typing import TypeVar @@ -164,7 +163,7 @@ def __init__( """ self.__handle = None self._context = get_default_context() if context is None else context - self._parameters: dict = {} + self._parameters: Dict[str, Parameter] = {} self._publishers: List[Publisher] = [] self._subscriptions: List[Subscription] = [] self._clients: List[Client] = [] @@ -179,8 +178,8 @@ def __init__( self._post_set_parameters_callbacks: List[Callable[[List[Parameter]], None]] = [] self._rate_group = ReentrantCallbackGroup() self._allow_undeclared_parameters = allow_undeclared_parameters - self._parameter_overrides: Dict[str, Parameter] = {} - self._descriptors = {} + self._parameter_overrides: Dict[str, Parameter[AllowableParameterValue]] = {} + self._descriptors: Dict[str, ParameterDescriptor] = {} namespace = namespace or '' if not self._context.ok(): @@ -357,12 +356,14 @@ def get_logger(self): return self._logger @overload - def declare_parameter(self, name: str, value: None = None, + def declare_parameter(self, name: str, + value: Union[None, Parameter.Type, ParameterValue] = None, descriptor: Optional[ParameterDescriptor] = None, ignore_override: bool = False) -> Parameter[None]: ... @overload - def declare_parameter(self, name: str, value: AllowableParameterValueT, + def declare_parameter(self, name: str, value: Union[AllowableParameterValueT, + Parameter.Type, ParameterValue], descriptor: Optional[ParameterDescriptor] = None, ignore_override: bool = False ) -> Parameter[AllowableParameterValueT]: ... @@ -370,7 +371,7 @@ def declare_parameter(self, name: str, value: AllowableParameterValueT, def declare_parameter( self, name: str, - value: AllowableParameterValue = None, + value: Union[AllowableParameterValue, Parameter.Type, ParameterValue] = None, descriptor: Optional[ParameterDescriptor] = None, ignore_override: bool = False ) -> Parameter: @@ -395,7 +396,8 @@ def declare_parameter( """ if value is None and descriptor is None: # Temporal patch so we get deprecation warning if only a name is provided. - args: Union[Tuple[str], Tuple[str, AllowableParameterValue, + args: Union[Tuple[str], Tuple[str, Union[AllowableParameterValue, + Parameter.Type, ParameterValue], ParameterDescriptor]] = (name, ) else: descriptor = ParameterDescriptor() if descriptor is None else descriptor @@ -408,7 +410,8 @@ def declare_parameters( parameters: List[Union[ Tuple[str], Tuple[str, Parameter.Type], - Tuple[str, AllowableParameterValue, ParameterDescriptor], + Tuple[str, Union[AllowableParameterValue, Parameter.Type, ParameterValue], + ParameterDescriptor], ]], ignore_override: bool = False ) -> List[Parameter]: @@ -483,9 +486,8 @@ def declare_parameters( # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. validate_parameter_name(name) - second_arg = parameter_tuple[1] if 1 < len(parameter_tuple) else None - descriptor = parameter_tuple[2] if 2 < len(parameter_tuple) else ParameterDescriptor() - + second_arg = parameter_tuple[1] if len(parameter_tuple) > 1 else None + descriptor = parameter_tuple[2] if len(parameter_tuple) > 2 else ParameterDescriptor() if not isinstance(descriptor, ParameterDescriptor): raise TypeError( f'Third element {descriptor} at index {index} in parameters list ' @@ -529,6 +531,10 @@ def declare_parameters( if not ignore_override and name in self._parameter_overrides: value = self._parameter_overrides[name].value + if isinstance(value, ParameterValue): + raise ValueError('Cannot declare a Parameter from a ParameterValue without it' + 'being included _parameter_overrides, and ignore_override=False') + parameter_list.append(Parameter(name, value=value)) descriptors.update({name: descriptor}) @@ -729,10 +735,7 @@ def get_parameter_or( return self._parameters[name] - def get_parameters_by_prefix(self, prefix: str) -> Dict[str, Optional[Union[ - bool, int, float, str, bytes, - Sequence[bool], Sequence[int], Sequence[float], Sequence[str] - ]]]: + def get_parameters_by_prefix(self, prefix: str) -> Dict[str, Parameter]: """ Get parameters that have a given prefix in their names as a dictionary. @@ -1049,7 +1052,7 @@ def _check_undeclared_parameters(self, parameter_list: List[Parameter]): if not self._allow_undeclared_parameters and any(undeclared_parameters): raise ParameterNotDeclaredException(list(undeclared_parameters)) - def _call_pre_set_parameters_callback(self, parameter_list: [List[Parameter]]): + def _call_pre_set_parameters_callback(self, parameter_list: List[Parameter]): if self._pre_set_parameters_callbacks: modified_parameter_list = [] for callback in self._pre_set_parameters_callbacks: @@ -1059,7 +1062,7 @@ def _call_pre_set_parameters_callback(self, parameter_list: [List[Parameter]]): else: return None - def _call_post_set_parameters_callback(self, parameter_list: [List[Parameter]]): + def _call_post_set_parameters_callback(self, parameter_list: List[Parameter]): if self._post_set_parameters_callbacks: for callback in self._post_set_parameters_callbacks: callback(parameter_list) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index ae4ca23c2..a741f2bb5 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import sys import array +import sys from enum import IntEnum from typing import Dict from typing import Generic @@ -41,7 +41,8 @@ list[float], Tuple[float, ...], array.array[float], list[str], Tuple[str, ...], array.array[str]] AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, - list[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], + list[bytes], Tuple[bytes, ...], list[bool], + Tuple[bool, ...], list[int], Tuple[int, ...], array.array[int], list[float], Tuple[float, ...], array.array[float], list[str], Tuple[str, ...], array.array[str]) @@ -54,10 +55,11 @@ List[str], Tuple[str, ...], 'array.array[str]'] AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], List[bool], Tuple[bool, ...], - List[int], Tuple[int, ...], 'array.array[int]', - List[float], Tuple[float, ...], 'array.array[float]', - List[str], Tuple[str, ...], 'array.array[str]') + List[bytes], Tuple[bytes, ...], List[bool], + Tuple[bool, ...], List[int], Tuple[int, ...], + 'array.array[int]', List[float], Tuple[float, ...], + 'array.array[float]', List[str], Tuple[str, ...], + 'array.array[str]') class Parameter(Generic[AllowableParameterValueT]): From 8c5651942c10c72ee75dc240dbe51fd8b978e456 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 21:14:41 -0400 Subject: [PATCH 15/23] Add non conflicting types back to constructor Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index a741f2bb5..206542609 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -174,7 +174,7 @@ def __init__(self, name: str, type_: Optional['Parameter.Type'] = None) -> None: def __init__(self, name: str, type_: Optional['Parameter.Type'], value: AllowableParameterValueT) -> None: ... - def __init__(self, name, type_=None, value=None): + def __init__(self, name: str, type_: Optional['Parameter.Type'] = None, value=None) -> None: if type_ is None: # This will raise a TypeError if it is not possible to get a type from the value. type_ = Parameter.Type.from_parameter_value(value) From 7f36fa23f98c698b223cbff6cdf5eb8a4f954232 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 21:21:26 -0400 Subject: [PATCH 16/23] Move sys import Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 206542609..a263296b0 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -13,8 +13,8 @@ # limitations under the License. import array -import sys from enum import IntEnum +import sys from typing import Dict from typing import Generic from typing import List @@ -32,7 +32,7 @@ PARAMETER_SEPARATOR_STRING = '.' # Mypy does not handle string literals of array.array[int/str/float] very well -# So if user has newer version of python can use proper arrays. +# So if user has newer version of python can use proper array types. if sys.version_info > (3, 9): AllowableParameterValue = Union[None, bool, int, float, str, list[bytes], Tuple[bytes, ...], From 5fba36dcab5f65892fd3ac9d58d71b7d92ee1a3b Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 21:23:47 -0400 Subject: [PATCH 17/23] Update error message Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index bd43f75a4..983b7af1e 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -532,8 +532,9 @@ def declare_parameters( value = self._parameter_overrides[name].value if isinstance(value, ParameterValue): - raise ValueError('Cannot declare a Parameter from a ParameterValue without it' - 'being included _parameter_overrides, and ignore_override=False') + raise ValueError('Cannot declare a Parameter from a ParameterValue without it ' + 'being included in self._parameter_overrides, and ', + 'ignore_override=False') parameter_list.append(Parameter(name, value=value)) descriptors.update({name: descriptor}) From f2466c1f6017c830f263170d13c9dad0bdd80b96 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 22:24:37 -0400 Subject: [PATCH 18/23] Add default value for generic Parameter Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 14 ++++----- rclpy/rclpy/parameter.py | 65 ++++++++++++++++++++++------------------ 2 files changed, 43 insertions(+), 36 deletions(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 983b7af1e..aa9bbb3e3 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -178,7 +178,7 @@ def __init__( self._post_set_parameters_callbacks: List[Callable[[List[Parameter]], None]] = [] self._rate_group = ReentrantCallbackGroup() self._allow_undeclared_parameters = allow_undeclared_parameters - self._parameter_overrides: Dict[str, Parameter[AllowableParameterValue]] = {} + self._parameter_overrides: Dict[str, Parameter] = {} self._descriptors: Dict[str, ParameterDescriptor] = {} namespace = namespace or '' @@ -355,12 +355,6 @@ def get_logger(self): """Get the nodes logger.""" return self._logger - @overload - def declare_parameter(self, name: str, - value: Union[None, Parameter.Type, ParameterValue] = None, - descriptor: Optional[ParameterDescriptor] = None, - ignore_override: bool = False) -> Parameter[None]: ... - @overload def declare_parameter(self, name: str, value: Union[AllowableParameterValueT, Parameter.Type, ParameterValue], @@ -368,6 +362,12 @@ def declare_parameter(self, name: str, value: Union[AllowableParameterValueT, ignore_override: bool = False ) -> Parameter[AllowableParameterValueT]: ... + @overload + def declare_parameter(self, name: str, + value: Union[None, Parameter.Type, ParameterValue] = None, + descriptor: Optional[ParameterDescriptor] = None, + ignore_override: bool = False) -> Parameter[None]: ... + def declare_parameter( self, name: str, diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index a263296b0..326822842 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -15,13 +15,14 @@ import array from enum import IntEnum import sys +from typing import Any from typing import Dict from typing import Generic from typing import List from typing import Optional from typing import overload from typing import Tuple -from typing import TypeVar +from typing import TYPE_CHECKING from typing import Union from rcl_interfaces.msg import Parameter as ParameterMsg @@ -31,35 +32,41 @@ PARAMETER_SEPARATOR_STRING = '.' -# Mypy does not handle string literals of array.array[int/str/float] very well -# So if user has newer version of python can use proper array types. -if sys.version_info > (3, 9): - AllowableParameterValue = Union[None, bool, int, float, str, - list[bytes], Tuple[bytes, ...], - list[bool], Tuple[bool, ...], - list[int], Tuple[int, ...], array.array[int], - list[float], Tuple[float, ...], array.array[float], - list[str], Tuple[str, ...], array.array[str]] - AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, - list[bytes], Tuple[bytes, ...], list[bool], - Tuple[bool, ...], - list[int], Tuple[int, ...], array.array[int], - list[float], Tuple[float, ...], array.array[float], - list[str], Tuple[str, ...], array.array[str]) +if TYPE_CHECKING: + from typing_extensions import TypeVar + # Mypy does not handle string literals of array.array[int/str/float] very well + # So if user has newer version of python can use proper array types. + if sys.version_info > (3, 9): + AllowableParameterValue = Union[None, bool, int, float, str, + list[bytes], Tuple[bytes, ...], + list[bool], Tuple[bool, ...], + list[int], Tuple[int, ...], array.array[int], + list[float], Tuple[float, ...], array.array[float], + list[str], Tuple[str, ...], array.array[str]] + AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, + str, list[bytes], Tuple[bytes, ...], list[bool], + Tuple[bool, ...], + list[int], Tuple[int, ...], array.array[int], + list[float], Tuple[float, ...], array.array[float], + list[str], Tuple[str, ...], array.array[str], + default=AllowableParameterValue) + else: + AllowableParameterValue = Union[None, bool, int, float, str, + List[bytes], Tuple[bytes, ...], + List[bool], Tuple[bool, ...], + List[int], Tuple[int, ...], 'array.array[int]', + List[float], Tuple[float, ...], 'array.array[float]', + List[str], Tuple[str, ...], 'array.array[str]'] + + AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, + str, List[bytes], Tuple[bytes, ...], List[bool], + Tuple[bool, ...], List[int], Tuple[int, ...], + 'array.array[int]', List[float], Tuple[float, ...], + 'array.array[float]', List[str], Tuple[str, ...], + 'array.array[str]', default=AllowableParameterValue) else: - AllowableParameterValue = Union[None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], - List[bool], Tuple[bool, ...], - List[int], Tuple[int, ...], 'array.array[int]', - List[float], Tuple[float, ...], 'array.array[float]', - List[str], Tuple[str, ...], 'array.array[str]'] - - AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, str, - List[bytes], Tuple[bytes, ...], List[bool], - Tuple[bool, ...], List[int], Tuple[int, ...], - 'array.array[int]', List[float], Tuple[float, ...], - 'array.array[float]', List[str], Tuple[str, ...], - 'array.array[str]') + AllowableParameterValue = Any + AllowableParameterValueT = TypeVar('AllowableParameterValueT') class Parameter(Generic[AllowableParameterValueT]): From f01ca94cc1379eb13b375b3543cc2eff5b740955 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 22:27:09 -0400 Subject: [PATCH 19/23] Add explanation comment Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 326822842..41363416f 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -65,6 +65,8 @@ 'array.array[float]', List[str], Tuple[str, ...], 'array.array[str]', default=AllowableParameterValue) else: + # Done to prevent runtime errors of undefined values. + # after python3.13 is minimum support this could be removed. AllowableParameterValue = Any AllowableParameterValueT = TypeVar('AllowableParameterValueT') From a646d8a961ce42fcb6ea668cd2ad5178033bc5a6 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 30 May 2024 22:32:30 -0400 Subject: [PATCH 20/23] Add TypeVar import inside else case Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 41363416f..9ae3625ec 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -65,6 +65,7 @@ 'array.array[float]', List[str], Tuple[str, ...], 'array.array[str]', default=AllowableParameterValue) else: + from typing import TypeVar # Done to prevent runtime errors of undefined values. # after python3.13 is minimum support this could be removed. AllowableParameterValue = Any From 6acc79c01c64b123857af50924ab7742a8754751 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 7 Jun 2024 16:19:34 -0400 Subject: [PATCH 21/23] push to rerun ci Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 9219c3e5e..96ecf301c 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -51,6 +51,7 @@ from rclpy.context import Context from rclpy.event_handler import PublisherEventCallbacks from rclpy.event_handler import SubscriptionEventCallbacks + from rclpy.exceptions import InvalidHandle from rclpy.exceptions import InvalidParameterTypeException from rclpy.exceptions import InvalidParameterValueException From 739b97ab049948da758edb55c412177d6792c141 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 7 Jun 2024 16:19:48 -0400 Subject: [PATCH 22/23] push to rerun ci Signed-off-by: Michael Carlstrom --- rclpy/rclpy/node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 96ecf301c..9219c3e5e 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -51,7 +51,6 @@ from rclpy.context import Context from rclpy.event_handler import PublisherEventCallbacks from rclpy.event_handler import SubscriptionEventCallbacks - from rclpy.exceptions import InvalidHandle from rclpy.exceptions import InvalidParameterTypeException from rclpy.exceptions import InvalidParameterValueException From ae10551bfc5289eb7c03987aefd8c30229a58e9d Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 12 Jun 2024 08:59:13 -0400 Subject: [PATCH 23/23] Switch back to union Signed-off-by: Michael Carlstrom --- rclpy/rclpy/parameter.py | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/rclpy/rclpy/parameter.py b/rclpy/rclpy/parameter.py index 9ae3625ec..464ee7065 100644 --- a/rclpy/rclpy/parameter.py +++ b/rclpy/rclpy/parameter.py @@ -43,13 +43,6 @@ list[int], Tuple[int, ...], array.array[int], list[float], Tuple[float, ...], array.array[float], list[str], Tuple[str, ...], array.array[str]] - AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, - str, list[bytes], Tuple[bytes, ...], list[bool], - Tuple[bool, ...], - list[int], Tuple[int, ...], array.array[int], - list[float], Tuple[float, ...], array.array[float], - list[str], Tuple[str, ...], array.array[str], - default=AllowableParameterValue) else: AllowableParameterValue = Union[None, bool, int, float, str, List[bytes], Tuple[bytes, ...], @@ -58,12 +51,9 @@ List[float], Tuple[float, ...], 'array.array[float]', List[str], Tuple[str, ...], 'array.array[str]'] - AllowableParameterValueT = TypeVar('AllowableParameterValueT', None, bool, int, float, - str, List[bytes], Tuple[bytes, ...], List[bool], - Tuple[bool, ...], List[int], Tuple[int, ...], - 'array.array[int]', List[float], Tuple[float, ...], - 'array.array[float]', List[str], Tuple[str, ...], - 'array.array[str]', default=AllowableParameterValue) + AllowableParameterValueT = TypeVar('AllowableParameterValueT', + bound=AllowableParameterValue, + default=AllowableParameterValue) else: from typing import TypeVar # Done to prevent runtime errors of undefined values.