From 1198c2448a79845a217572c3733fd449e39a7d0a Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 31 Jul 2024 11:50:52 -0400 Subject: [PATCH 01/28] Add types to wait_for_message.py Signed-off-by: Michael Carlstrom --- rclpy/rclpy/client.py | 5 ++ rclpy/rclpy/event_handler.py | 5 ++ rclpy/rclpy/guard_condition.py | 12 +++-- rclpy/rclpy/impl/implementation_singleton.py | 34 +++++++++--- rclpy/rclpy/service.py | 5 ++ rclpy/rclpy/subscription.py | 6 ++- rclpy/rclpy/timer.py | 5 ++ rclpy/rclpy/wait_for_message.py | 13 +++-- rclpy/rclpy/wait_set.py | 56 ++++++++++++++++++++ 9 files changed, 125 insertions(+), 16 deletions(-) create mode 100644 rclpy/rclpy/wait_set.py diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 99bdce9b3..2dd5c72e6 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -17,6 +17,7 @@ from types import TracebackType from typing import Dict from typing import Optional +from typing import Protocol from typing import Type from typing import TypeVar @@ -34,6 +35,10 @@ SrvTypeResponse = TypeVar('SrvTypeResponse') +class ClientHandle(Protocol): + pass + + class Client: def __init__( self, diff --git a/rclpy/rclpy/event_handler.py b/rclpy/rclpy/event_handler.py index d813117fd..bb3641b8d 100644 --- a/rclpy/rclpy/event_handler.py +++ b/rclpy/rclpy/event_handler.py @@ -16,6 +16,7 @@ from typing import Callable from typing import List from typing import Optional +from typing import Protocol import warnings import rclpy @@ -70,6 +71,10 @@ UnsupportedEventTypeError = _rclpy.UnsupportedEventTypeError +class EventHandle(Protocol): + pass + + class EventHandler(Waitable): """Waitable type to handle QoS events.""" diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index fb36923f6..49b62d8e2 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -15,7 +15,7 @@ from typing import Callable, Optional, Protocol from rclpy.callback_groups import CallbackGroup -from rclpy.context import Context +from rclpy.context import Context, ContextHandle from rclpy.destroyable import DestroyableType from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context @@ -23,8 +23,14 @@ class GuardConditionHandle(DestroyableType, Protocol): - def trigger_guard_condition(self) -> None: - ... + def __init__(self, context: ContextHandle) -> None: ... + + @property + def pointer(self) -> int: ... + """Get the address of the entity as an integer""" + + def trigger_guard_condition(self) -> None: ... + """Trigger a general purpose guard condition""" class GuardCondition: diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index cdbfc0b00..4cc86ac5d 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -27,18 +27,36 @@ """ -from typing import List, Protocol, Sequence +from typing import List, Protocol, Sequence, Type, TYPE_CHECKING from rpyutils import import_c_library -package = 'rclpy' -rclpy_implementation: 'rclpyHandle' = import_c_library('._rclpy_pybind11', package) +if TYPE_CHECKING: + from rclpy.client import ClientHandle + from rclpy.context import ContextHandle + from rclpy.event_handler import EventHandle + from rclpy.guard_condition import GuardConditionHandle + from rclpy.service import ServiceHandle + from rclpy.subscription import SubscriptionHandle + from rclpy.timer import TimerHandle + from rclpy.wait_set import WaitSetHandle + + class rclpyHandle(Protocol): + + def rclpy_remove_ros_args(self, pycli_args: Sequence[str]) -> List[str]: ... + def rclpy_get_rmw_implementation_identifier(self) -> str: ... -class rclpyHandle(Protocol): + Client: Type[ClientHandle] + Context: Type[ContextHandle] + EventHandle: Type[EventHandle] + GuardCondition: Type[GuardConditionHandle] + Service: Type[ServiceHandle] + Subscription: Type[SubscriptionHandle] + Timer: Type[TimerHandle] + WaitSet: Type[WaitSetHandle] - def rclpy_remove_ros_args(self, pycli_args: Sequence[str]) -> List[str]: - ... - def rclpy_get_rmw_implementation_identifier(self) -> str: - ... +package = 'rclpy' + +rclpy_implementation: 'rclpyHandle' = import_c_library('._rclpy_pybind11', package) diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index 7687530fd..fcfb8e1d4 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -15,6 +15,7 @@ from types import TracebackType from typing import Callable from typing import Optional +from typing import Protocol from typing import Type from typing import TypeVar @@ -30,6 +31,10 @@ SrvTypeResponse = TypeVar('SrvTypeResponse') +class ServiceHandle(Protocol): + pass + + class Service: def __init__( self, diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 131da3d30..3c045874a 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,7 +16,7 @@ from enum import Enum import inspect from types import TracebackType -from typing import Callable, Generic, List, Optional, Type, TypeVar +from typing import Callable, Generic, List, Optional, Protocol, Type, TypeVar from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import EventHandler, SubscriptionEventCallbacks @@ -29,6 +29,10 @@ MsgType = TypeVar('MsgType') +class SubscriptionHandle(Protocol): + pass + + class Subscription(Generic[MsgT]): class CallbackType(Enum): diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 236dcfb45..54150663d 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -17,6 +17,7 @@ from types import TracebackType from typing import Callable from typing import Optional +from typing import Protocol from typing import Type from rclpy.callback_groups import CallbackGroup @@ -60,6 +61,10 @@ def actual_call_time(self) -> Time: return self._actual_call_time +class TimerHandle(Protocol): + pass + + class Timer: def __init__( diff --git a/rclpy/rclpy/wait_for_message.py b/rclpy/rclpy/wait_for_message.py index a26eb7516..0d503fb79 100644 --- a/rclpy/rclpy/wait_for_message.py +++ b/rclpy/rclpy/wait_for_message.py @@ -12,23 +12,24 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Union +from typing import Literal, Tuple, Type, Union from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.signals import SignalHandlerGuardCondition +from rclpy.type_support import MsgT from rclpy.utilities import timeout_sec_to_nsec def wait_for_message( - msg_type, + msg_type: Type[MsgT], node: 'Node', topic: str, *, qos_profile: Union[QoSProfile, int] = 1, - time_to_wait=-1 -): + time_to_wait: Union[int, float] = -1 +) -> Union[Tuple[Literal[True], MsgT], Tuple[Literal[False], None]]: """ Wait for the next incoming message. @@ -41,6 +42,10 @@ def wait_for_message( could not be obtained or shutdown was triggered asynchronously on the context. """ context = node.context + + if context.handle is None: + raise Exception('Cannot create Waitset without a context.handle') + wait_set = _rclpy.WaitSet(1, 1, 0, 0, 0, 0, context.handle) wait_set.clear_entities() diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py new file mode 100644 index 000000000..60d024b9f --- /dev/null +++ b/rclpy/rclpy/wait_set.py @@ -0,0 +1,56 @@ +from typing import Literal, Protocol + +from rclpy.client import ClientHandle +from rclpy.context import ContextHandle +from rclpy.destroyable import DestroyableType +from rclpy.event_handler import EventHandle +from rclpy.guard_condition import GuardConditionHandle +from rclpy.service import ServiceHandle +from rclpy.subscription import SubscriptionHandle +from rclpy.timer import TimerHandle + + +IsReadyValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition', 'event'] +GetReadyEntityValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition'] + + +class WaitSetHandle(DestroyableType, Protocol): + + def __init__(self, number_of_subscriptions: int, number_of_guard_conditions: int, + number_of_timers: int, number_of_clients: int, number_of_services: int, + number_of_events: int, context: ContextHandle) -> None: ... + """Construct a WaitSet.""" + + @property + def pointer(self) -> int: ... + """Get the address of the entity as an integer""" + + def clear_entities(self) -> None: ... + """Clear all the pointers in the wait set""" + + def add_service(self, service: ServiceHandle) -> int: ... + """Add a service to the wait set structure""" + + def add_subscription(self, subscription: SubscriptionHandle) -> int: ... + """Add a subcription to the wait set structure""" + + def add_client(self, client: ClientHandle) -> int: ... + """Add a client to the wait set structure""" + + def add_guard_condition(self, guard_condition: GuardConditionHandle) -> int: ... + """Add a guard condition to the wait set structure""" + + def add_timer(self, timer: TimerHandle) -> int: ... + """Add a timer to the wait set structure""" + + def add_event(self, event: EventHandle) -> int: ... + """Add an event to the wait set structure""" + + def is_ready(self, entity_type: IsReadyValues, index: int) -> bool: ... + """Check if an entity in the wait set is ready by its index""" + + def get_ready_entities(self, entity_type: GetReadyEntityValues) -> list[int]: ... + """Get list of entities ready by entity type""" + + def wait(self, timeout: int) -> None: ... + """Wait until timeout is reached or event happened""" From a8199559ee8ff1b9fae00766e6575fb3fc264f61 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 31 Jul 2024 12:00:29 -0400 Subject: [PATCH 02/28] Add copyright Signed-off-by: Michael Carlstrom --- rclpy/rclpy/wait_set.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py index 60d024b9f..8aeac9b6f 100644 --- a/rclpy/rclpy/wait_set.py +++ b/rclpy/rclpy/wait_set.py @@ -1,3 +1,17 @@ +# 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 typing import Literal, Protocol from rclpy.client import ClientHandle From 6b52d12f9e36df45dd3c7698aa9588c862599129 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 31 Jul 2024 12:29:40 -0400 Subject: [PATCH 03/28] re-run CI Signed-off-by: Michael Carlstrom --- rclpy/rclpy/wait_set.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py index 8aeac9b6f..3c0c64fb2 100644 --- a/rclpy/rclpy/wait_set.py +++ b/rclpy/rclpy/wait_set.py @@ -24,6 +24,7 @@ from rclpy.timer import TimerHandle + IsReadyValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition', 'event'] GetReadyEntityValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition'] From a6072c1d0edc1094a5cf1b3f5e89eea456bd9236 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 31 Jul 2024 12:29:45 -0400 Subject: [PATCH 04/28] re-run CI Signed-off-by: Michael Carlstrom --- rclpy/rclpy/wait_set.py | 1 - 1 file changed, 1 deletion(-) diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py index 3c0c64fb2..8aeac9b6f 100644 --- a/rclpy/rclpy/wait_set.py +++ b/rclpy/rclpy/wait_set.py @@ -24,7 +24,6 @@ from rclpy.timer import TimerHandle - IsReadyValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition', 'event'] GetReadyEntityValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition'] From e39049abdb9b9ea529fe1e1111a7dc9b798a42ac Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 20:32:16 -0400 Subject: [PATCH 05/28] move Handles into _rclpy_pybind11 Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 289 ++++++++++++++++++- rclpy/rclpy/impl/implementation_singleton.py | 26 -- 2 files changed, 284 insertions(+), 31 deletions(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index df259e477..60bf8fad5 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -12,11 +12,290 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Sequence +from __future__ import annotations +from types import TracebackType +from typing import Any, Generic, Literal, Sequence, TypedDict -def rclpy_remove_ros_args(pycli_args: Sequence[str]) -> list[str]: ... -"""Remove ROS-specific arguments from argument vector.""" +from rclpy.clock import JumpHandle +from rclpy.clock_type import ClockType +from rclpy.qos import (QoSDurabilityPolicy, QoSHistoryPolicy, QoSLivelinessPolicy, + QoSReliabilityPolicy) +from rclpy.subscription import MessageInfo +from rclpy.type_support import MsgT -def rclpy_get_rmw_implementation_identifier() -> str: ... -"""Retrieve the identifier for the active RMW implementation.""" + +def rclpy_remove_ros_args(pycli_args: Sequence[str]) -> list[str]: + """Remove ROS-specific arguments from argument vector.""" + + +def rclpy_get_rmw_implementation_identifier() -> str: + """Retrieve the identifier for the active RMW implementation.""" + + +class Client: + pass + + +class rcl_time_point_t: + nanoseconds: int + clock_type: ClockType + + +class Destroyable: + + def __enter__(self) -> None: ... + + def __exit__(self, exc_type: type[BaseException] | None, + exc_val: BaseException | None, exctb: TracebackType | None) -> None: ... + + def destroy_when_not_in_use(self) -> None: + """Destroy the rcl object as soon as it's not actively being used.""" + + +class Clock(Destroyable): + + def __init__(self, clock_type: int) -> None: ... + + def get_now(self) -> rcl_time_point_t: + """Value of the clock.""" + + def get_ros_time_override_is_enabled(self) -> bool: + """Return if a clock using ROS time has the ROS time override enabled.""" + + def set_ros_time_override_is_enabled(self, enabled: bool) -> None: + """Set if a clock using ROS time has the ROS time override enabled.""" + + def set_ros_time_override(self, time_point: rcl_time_point_t) -> None: + """Set the ROS time override for a clock using ROS time.""" + + def add_clock_callback(self, pyjump_handle: JumpHandle, + on_clock_change: bool, min_forward: int, + min_backward: int) -> None: + """Add a time jump callback to a clock.""" + + def remove_clock_callback(self, pyjump_handle: JumpHandle) -> None: + """Remove a time jump callback from a clock.""" + + +class Context(Destroyable): + + def __init__(self, pyargs: list[str], domain_id: int) -> None: ... + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def get_domain_id(self) -> int: + """Retrieve domain id from init_options of context.""" + + def ok(self) -> bool: + """Status of the the client library.""" + + def shutdown(self) -> None: + """Shutdown context.""" + + +class rcl_duration_t: + nanoseconds: int + + +class EventHandle: + pass + + +class GuardCondition(Destroyable): + + def __init__(self, context: Context) -> None: ... + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def trigger_guard_condition(self) -> None: + """Trigger a general purpose guard condition.""" + + +class Service: + pass + + +class Subscription(Destroyable, Generic[MsgT]): + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def take_message(self, pymsg_type: type[MsgT], raw: bool) -> tuple[MsgT, MessageInfo]: + """Take a message and its metadata from a subscription.""" + + def get_logger_name(self) -> str: + """Get the name of the logger associated with the node of the subscription.""" + + def get_topic_name(self) -> str: + """Return the resolved topic name of a subscription.""" + + def get_publisher_count(self) -> int: + """Count the publishers from a subscription.""" + + +class Node: + pass + + +class Publisher(Destroyable, Generic[MsgT]): + + def __init__(self, arg0: Node, arg1: type[MsgT], arg2: str, arg3: object) -> None: + """Create _rclpy.Publisher.""" + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def get_logger_name(self) -> str: + """Get the name of the logger associated with the node of the publisher.""" + + def get_subscription_count(self) -> int: + """Count subscribers from a publisher.""" + + def get_topic_name(self) -> str: + """Retrieve the topic name from a Publisher.""" + + def publish(self, arg0: MsgT) -> None: + """Publish a message.""" + + def publish_raw(self, arg0: bytes) -> None: + """Publish a serialized message.""" + + def wait_for_all_acked(self, arg0: rcl_duration_t) -> bool: + """Wait until all published message data is acknowledged.""" + + +class TimeInfoDict(TypedDict): + expected_call_time: int + actual_call_time: int + + +class Timer(Destroyable): + + def __init__(self, clock: Clock, context: Context, period_nsec: int, + autostart: bool) -> None: ... + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def reset_timer(self) -> None: + """Reset a timer.""" + + def is_timer_ready(self) -> bool: + """Check if a timer as reached timeout.""" + + def call_timer(self) -> None: + """Call a timer and starts counting again.""" + + def call_timer_with_info(self) -> TimeInfoDict: + """Call a timer and starts counting again, retrieves actual and expected call time.""" + + def change_timer_period(self, period_nsec: int) -> None: + """Set the period of a timer.""" + + def time_until_next_call(self) -> int | None: + """Get the remaining time before timer is ready.""" + + def time_since_last_call(self) -> int: + """Get the elapsed time since last timer call.""" + + def get_timer_period(self) -> int: + """Get the period of a timer.""" + + def cancel_timer(self) -> None: + """Cancel a timer.""" + + def is_timer_canceled(self) -> bool: + """Check if a timer is canceled.""" + + +PredefinedQosProfileTNames = Literal['qos_profile_sensor_data', 'qos_profile_default', + 'qos_profile_system_default', 'qos_profile_services_default', + 'qos_profile_unknown', 'qos_profile_parameters', + 'qos_profile_parameter_events', 'qos_profile_best_available'] + + +class rmw_qos_profile_dict(TypedDict): + qos_history: QoSHistoryPolicy | int + qos_depth: int + qos_reliability: QoSReliabilityPolicy | int + qos_durability: QoSDurabilityPolicy | int + pyqos_lifespan: rcl_duration_t + pyqos_deadline: rcl_duration_t + qos_liveliness: QoSLivelinessPolicy | int + pyqos_liveliness_lease_duration: rcl_duration_t + avoid_ros_namespace_conventions: bool + + +class rmw_qos_profile_t: + + def __init__( + self, + qos_history: int, + qos_depth: int, + qos_reliability: int, + qos_durability: int, + pyqos_lifespan: rcl_duration_t, + pyqos_deadline: rcl_duration_t, + qos_liveliness: int, + pyqos_liveliness_lease_duration: rcl_duration_t, + avoid_ros_namespace_conventions: bool + ) -> None: ... + + def to_dict(self) -> rmw_qos_profile_dict: ... + + @staticmethod + def predefined(qos_profile_name: PredefinedQosProfileTNames) -> rmw_qos_profile_t: ... + + +IsReadyValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition', 'event'] +GetReadyEntityValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition'] + + +class WaitSet(Destroyable): + + def __init__(self, number_of_subscriptions: int, number_of_guard_conditions: int, + number_of_timers: int, number_of_clients: int, number_of_services: int, + number_of_events: int, context: Context) -> None: + """Construct a WaitSet.""" + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + + def clear_entities(self) -> None: + """Clear all the pointers in the wait set.""" + + def add_service(self, service: Service) -> int: + """Add a service to the wait set structure.""" + + def add_subscription(self, subscription: Subscription[Any]) -> int: + """Add a subcription to the wait set structure.""" + + def add_client(self, client: Client) -> int: + """Add a client to the wait set structure.""" + + def add_guard_condition(self, guard_condition: GuardCondition) -> int: + """Add a guard condition to the wait set structure.""" + + def add_timer(self, timer: Timer) -> int: + """Add a timer to the wait set structure.""" + + def add_event(self, event: EventHandle) -> int: + """Add an event to the wait set structure.""" + + def is_ready(self, entity_type: IsReadyValues, index: int) -> bool: + """Check if an entity in the wait set is ready by its index.""" + + def get_ready_entities(self, entity_type: GetReadyEntityValues) -> list[int]: + """Get list of entities ready by entity type.""" + + def wait(self, timeout: int) -> None: + """Wait until timeout is reached or event happened.""" diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index d1d4ce1f3..3e312280b 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -28,32 +28,6 @@ from rpyutils import import_c_library -if TYPE_CHECKING: - from rclpy.client import ClientHandle - from rclpy.context import ContextHandle - from rclpy.event_handler import EventHandle - from rclpy.guard_condition import GuardConditionHandle - from rclpy.service import ServiceHandle - from rclpy.subscription import SubscriptionHandle - from rclpy.timer import TimerHandle - from rclpy.wait_set import WaitSetHandle - - class rclpyHandle(Protocol): - - def rclpy_remove_ros_args(self, pycli_args: Sequence[str]) -> List[str]: ... - - def rclpy_get_rmw_implementation_identifier(self) -> str: ... - - Client: Type[ClientHandle] - Context: Type[ContextHandle] - EventHandle: Type[EventHandle] - GuardCondition: Type[GuardConditionHandle] - Service: Type[ServiceHandle] - Subscription: Type[SubscriptionHandle] - Timer: Type[TimerHandle] - WaitSet: Type[WaitSetHandle] - - package = 'rclpy' rclpy_implementation = import_c_library('._rclpy_pybind11', package) From 33cf1361c13822b1664b49127148a0b4ae79bee4 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 20:49:38 -0400 Subject: [PATCH 06/28] Move Handles into type stubs: Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 60bf8fad5..17a2435f2 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -100,8 +100,12 @@ class rcl_duration_t: nanoseconds: int -class EventHandle: - pass +class EventHandle(Destroyable): + + @property + def pointer(self) -> int: + """Get the address of the entity as an integer.""" + class GuardCondition(Destroyable): @@ -122,6 +126,9 @@ class Service: class Subscription(Destroyable, Generic[MsgT]): + def __init__(self, Node: Node, pymsg_type: type[MsgT], topic: str, + pyqos_profile: rmw_qos_profile_t) -> None: ... + @property def pointer(self) -> int: """Get the address of the entity as an integer.""" @@ -145,7 +152,7 @@ class Node: class Publisher(Destroyable, Generic[MsgT]): - def __init__(self, arg0: Node, arg1: type[MsgT], arg2: str, arg3: object) -> None: + def __init__(self, arg0: Node, arg1: type[MsgT], arg2: str, arg3: rmw_qos_profile_t) -> None: """Create _rclpy.Publisher.""" @property From 22edf2494e9e401059a8108e8f745a5cc02ed9ea Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 20:49:58 -0400 Subject: [PATCH 07/28] Move Handles into type stubs Signed-off-by: Michael Carlstrom --- rclpy/rclpy/client.py | 1 - rclpy/rclpy/clock.py | 38 ++---------------- rclpy/rclpy/context.py | 20 ++-------- rclpy/rclpy/destroyable.py | 29 -------------- rclpy/rclpy/duration.py | 12 ++---- rclpy/rclpy/event_handler.py | 12 +----- rclpy/rclpy/guard_condition.py | 21 ++-------- rclpy/rclpy/node.py | 7 +--- rclpy/rclpy/publisher.py | 40 ++----------------- rclpy/rclpy/qos.py | 18 ++------- rclpy/rclpy/service.py | 1 - rclpy/rclpy/subscription.py | 38 +++--------------- rclpy/rclpy/time.py | 23 ++++------- rclpy/rclpy/timer.py | 43 ++------------------- rclpy/rclpy/wait_set.py | 70 ---------------------------------- 15 files changed, 39 insertions(+), 334 deletions(-) delete mode 100644 rclpy/rclpy/destroyable.py delete mode 100644 rclpy/rclpy/wait_set.py diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 8f95aa6b1..cba396a1f 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -18,7 +18,6 @@ from typing import Dict from typing import Generic from typing import Optional -from typing import Protocol from typing import Type from typing import TypeVar diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index 712ae4f79..3b9f363e8 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -14,16 +14,15 @@ from enum import IntEnum from types import TracebackType -from typing import Callable, Optional, Protocol, Type, TYPE_CHECKING, TypedDict +from typing import Callable, Optional, Type, TYPE_CHECKING, TypedDict from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from .clock_type import ClockType from .context import Context -from .destroyable import DestroyableType from .duration import Duration from .exceptions import NotInitializedException -from .time import Time, TimeHandle +from .time import Time from .utilities import get_default_context @@ -146,39 +145,10 @@ def __exit__(self, t: Optional[Type[BaseException]], self.unregister() -class ClockHandle(DestroyableType, Protocol): - """Generic alias of _rclpy.Clock.""" - - def get_now(self) -> TimeHandle: - """Value of the clock.""" - ... - - def get_ros_time_override_is_enabled(self) -> bool: - """Return if a clock using ROS time has the ROS time override enabled.""" - ... - - def set_ros_time_override_is_enabled(self, enabled: bool) -> None: - """Set if a clock using ROS time has the ROS time override enabled.""" - ... - - def set_ros_time_override(self, time_point: TimeHandle) -> None: - """Set the ROS time override for a clock using ROS time.""" - ... - - def add_clock_callback(self, pyjump_handle: JumpHandle, - on_clock_change: bool, min_forward: int, min_backward: int) -> None: - """Add a time jump callback to a clock.""" - ... - - def remove_clock_callback(self, pyjump_handle: JumpHandle) -> None: - """Remove a time jump callback from a clock.""" - ... - - class Clock: if TYPE_CHECKING: - __clock: ClockHandle + __clock: _rclpy.Clock _clock_type: ClockType def __new__(cls, *, @@ -198,7 +168,7 @@ def clock_type(self) -> ClockType: return self._clock_type @property - def handle(self) -> ClockHandle: + def handle(self) -> _rclpy.Clock: """ Return the internal instance of ``rclpy::Clock``. diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index 5563b09b1..9919867a8 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -20,31 +20,17 @@ from typing import ContextManager from typing import List from typing import Optional -from typing import Protocol from typing import Type from typing import TYPE_CHECKING from typing import Union import warnings import weakref -from rclpy.destroyable import DestroyableType +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy if TYPE_CHECKING: from rclpy.node import Node - -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: int = 0 @@ -68,11 +54,11 @@ def __init__(self) -> None: self._lock = threading.RLock() self._callbacks: List[Union['weakref.WeakMethod[MethodType]', Callable[[], None]]] = [] self._logging_initialized = False - self.__context: Optional[ContextHandle] = None + self.__context: Optional[_rclpy.Context] = None self.__node_weak_ref_list: List[weakref.ReferenceType['Node']] = [] @property - def handle(self) -> Optional[ContextHandle]: + def handle(self) -> Optional[_rclpy.Context]: return self.__context def destroy(self) -> None: diff --git a/rclpy/rclpy/destroyable.py b/rclpy/rclpy/destroyable.py deleted file mode 100644 index 8c057923d..000000000 --- a/rclpy/rclpy/destroyable.py +++ /dev/null @@ -1,29 +0,0 @@ -# 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: - ... diff --git a/rclpy/rclpy/duration.py b/rclpy/rclpy/duration.py index c3356cb07..ac4655bca 100644 --- a/rclpy/rclpy/duration.py +++ b/rclpy/rclpy/duration.py @@ -12,19 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Protocol, Union +from typing import Union import builtin_interfaces.msg from rclpy.constants import S_TO_NS from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -class DurationHandle(Protocol): - """Type alias of _rclpy.rcl_duration_t.""" - - nanoseconds: int - - class Duration: """A period between two time points, with nanosecond precision.""" @@ -41,7 +35,7 @@ def __init__(self, *, seconds: Union[int, float] = 0, nanoseconds: int = 0): # pybind11 would raise TypeError, but we want OverflowError raise OverflowError( 'Total nanoseconds value is too large to store in C duration.') - self._duration_handle: DurationHandle = _rclpy.rcl_duration_t(total_nanoseconds) + self._duration_handle = _rclpy.rcl_duration_t(total_nanoseconds) @property def nanoseconds(self) -> int: @@ -106,7 +100,7 @@ def from_msg(cls, msg: builtin_interfaces.msg.Duration) -> 'Duration': raise TypeError('Must pass a builtin_interfaces.msg.Duration object') return cls(seconds=msg.sec, nanoseconds=msg.nanosec) - def get_c_duration(self) -> DurationHandle: + def get_c_duration(self) -> _rclpy.rcl_duration_t: return self._duration_handle diff --git a/rclpy/rclpy/event_handler.py b/rclpy/rclpy/event_handler.py index b7880f92f..e3267edce 100644 --- a/rclpy/rclpy/event_handler.py +++ b/rclpy/rclpy/event_handler.py @@ -16,8 +16,6 @@ from typing import Callable from typing import List from typing import Optional -from typing import Protocol -from typing import TYPE_CHECKING import warnings import rclpy @@ -29,10 +27,6 @@ from rclpy.waitable import Waitable -if TYPE_CHECKING: - from rclpy.subscription import SubscriptionHandle - - QoSPublisherEventType = _rclpy.rcl_publisher_event_type_t QoSSubscriptionEventType = _rclpy.rcl_subscription_event_type_t @@ -76,10 +70,6 @@ UnsupportedEventTypeError = _rclpy.UnsupportedEventTypeError -class EventHandle(Protocol): - pass - - class EventHandler(Waitable): """Waitable type to handle QoS events.""" @@ -199,7 +189,7 @@ def __init__( self.use_default_callbacks = use_default_callbacks def create_event_handlers( - self, callback_group: CallbackGroup, subscription: 'SubscriptionHandle', + self, callback_group: CallbackGroup, subscription: '_rclpy.Subscription', topic_name: str) -> List[EventHandler]: with subscription: logger = get_logger(subscription.get_logger_name()) diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index 49b62d8e2..416cf1829 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -12,27 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Callable, Optional, Protocol +from typing import Callable, Optional from rclpy.callback_groups import CallbackGroup -from rclpy.context import Context, ContextHandle -from rclpy.destroyable import DestroyableType +from rclpy.context import Context from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context -class GuardConditionHandle(DestroyableType, Protocol): - - def __init__(self, context: ContextHandle) -> None: ... - - @property - def pointer(self) -> int: ... - """Get the address of the entity as an integer""" - - def trigger_guard_condition(self) -> None: ... - """Trigger a general purpose guard condition""" - - class GuardCondition: def __init__(self, callback: Optional[Callable], @@ -50,7 +37,7 @@ def __init__(self, callback: Optional[Callable], raise RuntimeError('Context must be initialized a GuardCondition can be created') with self._context.handle: - self.__gc: GuardConditionHandle = _rclpy.GuardCondition(self._context.handle) + self.__gc = _rclpy.GuardCondition(self._context.handle) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor @@ -63,7 +50,7 @@ def trigger(self) -> None: self.__gc.trigger_guard_condition() @property - def handle(self) -> GuardConditionHandle: + def handle(self) -> _rclpy.GuardCondition: return self.__gc def destroy(self) -> None: diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 66c669ebb..368f43c79 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -78,7 +78,6 @@ from rclpy.service import Service from rclpy.subscription import MessageInfo from rclpy.subscription import Subscription -from rclpy.subscription import SubscriptionHandle from rclpy.time_source import TimeSource from rclpy.timer import Rate from rclpy.timer import Timer, TimerInfo @@ -113,10 +112,6 @@ NodeNameNonExistentError = _rclpy.NodeNameNonExistentError -class NodeHandle: - pass - - class Node: """ A Node in the ROS graph. @@ -1657,7 +1652,7 @@ def create_subscription( check_is_valid_msg_type(msg_type) try: with self.handle: - subscription_object: SubscriptionHandle[MsgT] = _rclpy.Subscription( + subscription_object = _rclpy.Subscription( self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index 41835d493..06003f154 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -13,11 +13,10 @@ # limitations under the License. from types import TracebackType -from typing import Generic, List, Optional, Protocol, Type, TYPE_CHECKING, TypeVar, Union +from typing import Generic, List, Optional, Type, TypeVar, Union from rclpy.callback_groups import CallbackGroup -from rclpy.destroyable import DestroyableType -from rclpy.duration import Duration, DurationHandle +from rclpy.duration import Duration from rclpy.event_handler import EventHandler, PublisherEventCallbacks from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile @@ -26,43 +25,12 @@ # Left to support Legacy TypeVars. MsgType = TypeVar('MsgType') -if TYPE_CHECKING: - from rclpy.node import NodeHandle - - -class PublisherHandle(DestroyableType, Protocol[MsgT]): - - def __init__(self, arg0: 'NodeHandle', arg1: Type[MsgT], arg2: str, arg3: object) -> None: - """Create PublisherHandle.""" - - @property - def pointer(self) -> int: - """Get the address of the entity as an integer.""" - - def get_logger_name(self) -> str: - """Get the name of the logger associated with the node of the publisher.""" - - def get_subscription_count(self) -> int: - """Count subscribers from a publisher.""" - - def get_topic_name(self) -> str: - """Retrieve the topic name from a Publisher.""" - - def publish(self, arg0: MsgT) -> None: - """Publish a message.""" - - def publish_raw(self, arg0: bytes) -> None: - """Publish a serialized message.""" - - def wait_for_all_acked(self, arg0: DurationHandle) -> bool: - """Wait until all published message data is acknowledged.""" - class Publisher(Generic[MsgT]): def __init__( self, - publisher_impl: PublisherHandle[MsgT], + publisher_impl: _rclpy.Publisher[MsgT], msg_type: Type[MsgT], topic: str, qos_profile: QoSProfile, @@ -118,7 +86,7 @@ def topic_name(self) -> str: return self.__publisher.get_topic_name() @property - def handle(self) -> PublisherHandle[MsgT]: + def handle(self) -> _rclpy.Publisher[MsgT]: return self.__publisher def destroy(self) -> None: diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index ba549432b..6c8b18f92 100644 --- a/rclpy/rclpy/qos.py +++ b/rclpy/rclpy/qos.py @@ -13,11 +13,11 @@ # limitations under the License. from enum import Enum, IntEnum -from typing import (Callable, Iterable, List, Optional, Protocol, Tuple, Type, TYPE_CHECKING, +from typing import (Callable, Iterable, List, Optional, Tuple, Type, TYPE_CHECKING, TypedDict, TypeVar, Union) import warnings -from rclpy.duration import Duration, DurationHandle +from rclpy.duration import Duration from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy if TYPE_CHECKING: @@ -56,18 +56,6 @@ def __init__(self, message: str) -> None: Exception(self, f'Invalid QoSProfile: {message}') -class QoSProfileHandle(Protocol): - qos_history: Union['QoSHistoryPolicy', int] - qos_depth: int - qos_reliability: Union['QoSReliabilityPolicy', int] - qos_durability: Union['QoSDurabilityPolicy', int] - pyqos_lifespan: DurationHandle - pyqos_deadline: DurationHandle - qos_liveliness: Union['QoSLivelinessPolicy', int] - pyqos_liveliness_lease_duration: DurationHandle - avoid_ros_namespace_conventions: bool - - class QoSProfileDictionary(TypedDict): history: 'QoSHistoryPolicy' depth: int @@ -276,7 +264,7 @@ def avoid_ros_namespace_conventions(self, value: bool) -> None: assert isinstance(value, bool) self._avoid_ros_namespace_conventions = value - def get_c_qos_profile(self) -> QoSProfileHandle: + def get_c_qos_profile(self) -> _rclpy.rmw_qos_profile_t: return _rclpy.rmw_qos_profile_t( self.history, self.depth, diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index ffe6ad902..bc13940cf 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -16,7 +16,6 @@ from typing import Callable from typing import Generic from typing import Optional -from typing import Protocol from typing import Type from typing import TypeVar diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 37b7e996c..332705092 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,11 +16,11 @@ from enum import Enum import inspect from types import TracebackType -from typing import Callable, Generic, Optional, Protocol, Tuple, Type, TypedDict, TypeVar, Union +from typing import Callable, Generic, Optional, Type, TypedDict, TypeVar, Union from rclpy.callback_groups import CallbackGroup -from rclpy.destroyable import DestroyableType from rclpy.event_handler import SubscriptionEventCallbacks +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.type_support import MsgT @@ -32,38 +32,10 @@ class MessageInfo(TypedDict): reception_sequence_number: Optional[int] -class SubscriptionHandle(DestroyableType, Protocol[MsgT]): - - @property - def pointer(self) -> int: - """Get the address of the entity as an integer.""" - ... - - def take_message(self, pymsg_type: Type[MsgT], raw: bool) -> Tuple[MsgT, MessageInfo]: - """Take a message and its metadata from a subscription.""" - ... - - def get_logger_name(self) -> str: - """Get the name of the logger associated with the node of the subscription.""" - ... - - def get_topic_name(self) -> str: - """Return the resolved topic name of a subscription.""" - ... - - def get_publisher_count(self) -> int: - """Count the publishers from a subscription.""" - ... - - # Left to support Legacy TypeVars. MsgType = TypeVar('MsgType') -class SubscriptionHandle(Protocol): - pass - - class Subscription(Generic[MsgT]): class CallbackType(Enum): @@ -72,7 +44,7 @@ class CallbackType(Enum): def __init__( self, - subscription_impl: SubscriptionHandle[MsgT], + subscription_impl: _rclpy.Subscription[MsgT], msg_type: Type[MsgT], topic: str, callback: Union[Callable[[MsgT], None], Callable[[MsgT, MessageInfo], None]], @@ -118,7 +90,7 @@ def get_publisher_count(self) -> int: return self.__subscription.get_publisher_count() @property - def handle(self) -> SubscriptionHandle[MsgT]: + def handle(self) -> _rclpy.Subscription[MsgT]: return self.__subscription def destroy(self) -> None: @@ -161,7 +133,7 @@ def callback(self, value: Union[Callable[[MsgT], None], 'Subscription.__init__(): callback should be either be callable with one argument' '(to get only the message) or two (to get message and message info)') - def __enter__(self) -> 'Subscription': + def __enter__(self) -> 'Subscription[MsgT]': return self def __exit__( diff --git a/rclpy/rclpy/time.py b/rclpy/rclpy/time.py index 57693a00b..a6f89aaba 100644 --- a/rclpy/rclpy/time.py +++ b/rclpy/rclpy/time.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import overload, Protocol, Tuple, Union +from typing import overload, Tuple, Union import builtin_interfaces.msg @@ -23,13 +23,6 @@ from .clock_type import ClockType -class TimeHandle(Protocol): - """Type alias of _rclpy.rcl_time_point_t.""" - - nanoseconds: int - clock_type: ClockType - - class Time: """ Represents a point in time. @@ -56,7 +49,7 @@ def __init__( # pybind11 would raise TypeError, but we want OverflowError raise OverflowError( 'Total nanoseconds value is too large to store in C time point.') - self._time_handle: TimeHandle = _rclpy.rcl_time_point_t(total_nanoseconds, clock_type) + self._time_handle = _rclpy.rcl_time_point_t(total_nanoseconds, clock_type) @property def nanoseconds(self) -> int: @@ -119,40 +112,40 @@ def __sub__(self, other: Union['Time', Duration]) -> Union['Time', Duration]: else: return NotImplemented - def __eq__(self, other: object) -> bool: + def __eq__(self, other: 'Time') -> bool: if isinstance(other, Time): if self.clock_type != other.clock_type: raise TypeError("Can't compare times with different clock types") return self.nanoseconds == other.nanoseconds return NotImplemented - def __ne__(self, other: object) -> bool: + def __ne__(self, other: 'Time') -> bool: if isinstance(other, Time): return not self.__eq__(other) return NotImplemented - def __lt__(self, other: object) -> bool: + def __lt__(self, other: 'Time') -> bool: if isinstance(other, Time): if self.clock_type != other.clock_type: raise TypeError("Can't compare times with different clock types") return self.nanoseconds < other.nanoseconds return NotImplemented - def __le__(self, other: object) -> bool: + def __le__(self, other: 'Time') -> bool: if isinstance(other, Time): if self.clock_type != other.clock_type: raise TypeError("Can't compare times with different clock types") return self.nanoseconds <= other.nanoseconds return NotImplemented - def __gt__(self, other: object) -> bool: + def __gt__(self, other: 'Time') -> bool: if isinstance(other, Time): if self.clock_type != other.clock_type: raise TypeError("Can't compare times with different clock types") return self.nanoseconds > other.nanoseconds return NotImplemented - def __ge__(self, other: object) -> bool: + def __ge__(self, other: 'Time') -> bool: if isinstance(other, Time): if self.clock_type != other.clock_type: raise TypeError("Can't compare times with different clock types") diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 0b2e200db..c5b577053 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -17,7 +17,6 @@ from types import TracebackType from typing import Callable from typing import Optional -from typing import Protocol from typing import Type from typing import Union @@ -25,44 +24,12 @@ from rclpy.clock import Clock from rclpy.clock_type import ClockType from rclpy.context import Context -from rclpy.destroyable import DestroyableType from rclpy.exceptions import InvalidHandle, ROSInterruptException from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.time import Time from rclpy.utilities import get_default_context -class TimerHandle(DestroyableType, Protocol): - """Type alias of _rclpy.Timer.""" - - def reset_timer(self) -> None: - ... - - def is_timer_ready(self) -> bool: - ... - - def call_timer(self) -> None: - ... - - def change_timer_period(self, period_nsec: int) -> None: - ... - - def time_until_next_call(self) -> Optional[int]: - ... - - def time_since_last_call(self) -> int: - ... - - def get_timer_period(self) -> int: - ... - - def cancel_timer(self) -> None: - ... - - def is_timer_canceled(self) -> bool: - ... - - class TimerInfo: """ Represents a timer call information. @@ -95,10 +62,6 @@ def actual_call_time(self) -> Time: return self._actual_call_time -class TimerHandle(Protocol): - pass - - class Timer: def __init__( @@ -134,9 +97,9 @@ def __init__( self._context = get_default_context() if context is None else context self._clock = clock if self._context.handle is None: - raise RuntimeError('Context must be initialized before create a TimerHandle.') + raise RuntimeError('Context must be initialized before create a _rclpy.Timer.') with self._clock.handle, self._context.handle: - self.__timer: TimerHandle = _rclpy.Timer( + self.__timer = _rclpy.Timer( self._clock.handle, self._context.handle, timer_period_ns, autostart) self.timer_period_ns = timer_period_ns self.callback = callback @@ -145,7 +108,7 @@ def __init__( self._executor_event = False @property - def handle(self) -> TimerHandle: + def handle(self) -> _rclpy.Timer: return self.__timer def destroy(self) -> None: diff --git a/rclpy/rclpy/wait_set.py b/rclpy/rclpy/wait_set.py deleted file mode 100644 index 8aeac9b6f..000000000 --- a/rclpy/rclpy/wait_set.py +++ /dev/null @@ -1,70 +0,0 @@ -# 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 typing import Literal, Protocol - -from rclpy.client import ClientHandle -from rclpy.context import ContextHandle -from rclpy.destroyable import DestroyableType -from rclpy.event_handler import EventHandle -from rclpy.guard_condition import GuardConditionHandle -from rclpy.service import ServiceHandle -from rclpy.subscription import SubscriptionHandle -from rclpy.timer import TimerHandle - - -IsReadyValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition', 'event'] -GetReadyEntityValues = Literal['subscription', 'client', 'service', 'timer', 'guard_condition'] - - -class WaitSetHandle(DestroyableType, Protocol): - - def __init__(self, number_of_subscriptions: int, number_of_guard_conditions: int, - number_of_timers: int, number_of_clients: int, number_of_services: int, - number_of_events: int, context: ContextHandle) -> None: ... - """Construct a WaitSet.""" - - @property - def pointer(self) -> int: ... - """Get the address of the entity as an integer""" - - def clear_entities(self) -> None: ... - """Clear all the pointers in the wait set""" - - def add_service(self, service: ServiceHandle) -> int: ... - """Add a service to the wait set structure""" - - def add_subscription(self, subscription: SubscriptionHandle) -> int: ... - """Add a subcription to the wait set structure""" - - def add_client(self, client: ClientHandle) -> int: ... - """Add a client to the wait set structure""" - - def add_guard_condition(self, guard_condition: GuardConditionHandle) -> int: ... - """Add a guard condition to the wait set structure""" - - def add_timer(self, timer: TimerHandle) -> int: ... - """Add a timer to the wait set structure""" - - def add_event(self, event: EventHandle) -> int: ... - """Add an event to the wait set structure""" - - def is_ready(self, entity_type: IsReadyValues, index: int) -> bool: ... - """Check if an entity in the wait set is ready by its index""" - - def get_ready_entities(self, entity_type: GetReadyEntityValues) -> list[int]: ... - """Get list of entities ready by entity type""" - - def wait(self, timeout: int) -> None: ... - """Wait until timeout is reached or event happened""" From 90f151841e31f9853ca0eb861878343fdfa3a2d9 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 20:59:12 -0400 Subject: [PATCH 08/28] move [] into string Signed-off-by: Michael Carlstrom --- rclpy/rclpy/subscription.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 332705092..2675f9134 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -20,9 +20,9 @@ from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import SubscriptionEventCallbacks -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile -from rclpy.type_support import MsgT +from .type_support import MsgT class MessageInfo(TypedDict): @@ -44,7 +44,7 @@ class CallbackType(Enum): def __init__( self, - subscription_impl: _rclpy.Subscription[MsgT], + subscription_impl: '_rclpy.Subscription[MsgT]', msg_type: Type[MsgT], topic: str, callback: Union[Callable[[MsgT], None], Callable[[MsgT, MessageInfo], None]], @@ -90,7 +90,7 @@ def get_publisher_count(self) -> int: return self.__subscription.get_publisher_count() @property - def handle(self) -> _rclpy.Subscription[MsgT]: + def handle(self) -> '_rclpy.Subscription[MsgT]': return self.__subscription def destroy(self) -> None: From 1019bcc9b4340a000cbf1d3de524004e1fb7299e Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 21:04:56 -0400 Subject: [PATCH 09/28] fix imports Signed-off-by: Michael Carlstrom --- rclpy/rclpy/subscription.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 2675f9134..86eedb0a9 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -20,9 +20,9 @@ from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import SubscriptionEventCallbacks -from impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile -from .type_support import MsgT +from rclpy.type_support import MsgT class MessageInfo(TypedDict): From 7f22be99acc84f4035d7ef3da0b11b46e522b778 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 21:08:56 -0400 Subject: [PATCH 10/28] remove extra line Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 1 - 1 file changed, 1 deletion(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 17a2435f2..5a421204d 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -107,7 +107,6 @@ class EventHandle(Destroyable): """Get the address of the entity as an integer.""" - class GuardCondition(Destroyable): def __init__(self, context: Context) -> None: ... From a61090f90ca89faa50a6cb7764f24573ee437747 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 21:16:10 -0400 Subject: [PATCH 11/28] puy _rclpy.Publisher in quotes Signed-off-by: Michael Carlstrom --- rclpy/rclpy/publisher.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index 06003f154..66797f2c4 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -30,7 +30,7 @@ class Publisher(Generic[MsgT]): def __init__( self, - publisher_impl: _rclpy.Publisher[MsgT], + publisher_impl: '_rclpy.Publisher[MsgT]', msg_type: Type[MsgT], topic: str, qos_profile: QoSProfile, @@ -86,7 +86,7 @@ def topic_name(self) -> str: return self.__publisher.get_topic_name() @property - def handle(self) -> _rclpy.Publisher[MsgT]: + def handle(self) -> '_rclpy.Publisher[MsgT]': return self.__publisher def destroy(self) -> None: From c18a10ac3ca66b54a030ff8e96f316f7d18f188d Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 21:29:04 -0400 Subject: [PATCH 12/28] fix capitalization Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 5a421204d..71ff6b724 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -125,7 +125,7 @@ class Service: class Subscription(Destroyable, Generic[MsgT]): - def __init__(self, Node: Node, pymsg_type: type[MsgT], topic: str, + def __init__(self, node: Node, pymsg_type: type[MsgT], topic: str, pyqos_profile: rmw_qos_profile_t) -> None: ... @property From e45fd69bd337af373785de8348ffecb91d581e6a Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 9 Aug 2024 22:32:23 -0400 Subject: [PATCH 13/28] Add EventHandle Constructor Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 30 +++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 71ff6b724..75427df4d 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -14,8 +14,9 @@ from __future__ import annotations +from enum import IntEnum from types import TracebackType -from typing import Any, Generic, Literal, Sequence, TypedDict +from typing import Any, Generic, Literal, overload, Sequence, TypedDict from rclpy.clock import JumpHandle from rclpy.clock_type import ClockType @@ -100,12 +101,39 @@ class rcl_duration_t: nanoseconds: int +class rcl_subscription_event_type_t(IntEnum): + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED: int + RCL_SUBSCRIPTION_LIVELINESS_CHANGED: int + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS: int + RCL_SUBSCRIPTION_MESSAGE_LOST: int + RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE: int + RCL_SUBSCRIPTION_MATCHED: int + + +class rcl_publisher_event_type_t(IntEnum): + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED: int + RCL_PUBLISHER_LIVELINESS_LOST: int + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS: int + RCL_PUBLISHER_INCOMPATIBLE_TYPE: int + RCL_PUBLISHER_MATCHED: int + + class EventHandle(Destroyable): + @overload + def __init__(self, subcription: Subscription, + event_type: rcl_subscription_event_type_t) -> None: ... + + @overload + def __init__(self, publisher: Publisher, event_type: rcl_publisher_event_type_t) -> None: ... + @property def pointer(self) -> int: """Get the address of the entity as an integer.""" + def take_event(self) -> Any | None: + """Get pending data from a ready event.""" + class GuardCondition(Destroyable): From e0103780c316a0db0460fdb92d66c5be1e11e634 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Wed, 21 Aug 2024 02:53:24 -0400 Subject: [PATCH 14/28] Use RuntimeError for context Signed-off-by: Michael Carlstrom --- rclpy/rclpy/wait_for_message.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/wait_for_message.py b/rclpy/rclpy/wait_for_message.py index 0d503fb79..82302fff9 100644 --- a/rclpy/rclpy/wait_for_message.py +++ b/rclpy/rclpy/wait_for_message.py @@ -44,7 +44,7 @@ def wait_for_message( context = node.context if context.handle is None: - raise Exception('Cannot create Waitset without a context.handle') + raise RuntimeError('Cannot create Waitset without a context.handle') wait_set = _rclpy.WaitSet(1, 1, 0, 0, 0, 0, context.handle) wait_set.clear_entities() From 47c43f5633ff0c05819c3997ea183f2407e505ca Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 11:19:38 -0400 Subject: [PATCH 15/28] Add TYPE_CHECKING import Signed-off-by: Michael Carlstrom --- rclpy/rclpy/event_handler.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/rclpy/event_handler.py b/rclpy/rclpy/event_handler.py index ee30b71a5..e846b9135 100644 --- a/rclpy/rclpy/event_handler.py +++ b/rclpy/rclpy/event_handler.py @@ -17,6 +17,7 @@ from typing import Callable from typing import List from typing import Optional +from typing import TYPE_CHECKING import warnings import rclpy From 19e7afc43cd3e73c57794323d70f747a6cb7cfdd Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 11:58:54 -0400 Subject: [PATCH 16/28] start lifecycle Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/implementation_singleton.pyi | 2 +- rclpy/rclpy/lifecycle/managed_entity.py | 27 +++++++++++-------- rclpy/rclpy/lifecycle/node.py | 23 +++++++++------- rclpy/rclpy/lifecycle/publisher.py | 2 +- 4 files changed, 31 insertions(+), 23 deletions(-) diff --git a/rclpy/rclpy/impl/implementation_singleton.pyi b/rclpy/rclpy/impl/implementation_singleton.pyi index a1e16bdf9..521e06e98 100644 --- a/rclpy/rclpy/impl/implementation_singleton.pyi +++ b/rclpy/rclpy/impl/implementation_singleton.pyi @@ -13,6 +13,6 @@ # limitations under the License. -from rclpy.impl import _rclpy_pybind11 +from impl import _rclpy_pybind11 rclpy_implementation = _rclpy_pybind11 diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py index 93aaf9b62..7115a91ac 100644 --- a/rclpy/rclpy/lifecycle/managed_entity.py +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -12,37 +12,42 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import TYPE_CHECKING from functools import wraps from ..impl.implementation_singleton import rclpy_implementation as _rclpy +if TYPE_CHECKING: + from typing import TypeAlias + from rclpy.lifecycle.node import LifecycleState -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType + +TransitionCallbackReturn: 'TypeAlias' = _rclpy.TransitionCallbackReturnType class ManagedEntity: - def on_configure(self, state) -> TransitionCallbackReturn: + def on_configure(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle configure transition request.""" return TransitionCallbackReturn.SUCCESS - def on_cleanup(self, state) -> TransitionCallbackReturn: + def on_cleanup(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle cleanup transition request.""" return TransitionCallbackReturn.SUCCESS - def on_shutdown(self, state) -> TransitionCallbackReturn: + def on_shutdown(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle shutdown transition request.""" return TransitionCallbackReturn.SUCCESS - def on_activate(self, state) -> TransitionCallbackReturn: + def on_activate(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle activate transition request.""" return TransitionCallbackReturn.SUCCESS - def on_deactivate(self, state) -> TransitionCallbackReturn: + def on_deactivate(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle deactivate transition request.""" return TransitionCallbackReturn.SUCCESS - def on_error(self, state) -> TransitionCallbackReturn: + def on_error(self, state: 'LifecycleState') -> TransitionCallbackReturn: """Handle error transition request.""" return TransitionCallbackReturn.SUCCESS @@ -50,19 +55,19 @@ def on_error(self, state) -> TransitionCallbackReturn: class SimpleManagedEntity(ManagedEntity): """A simple managed entity that only sets a flag when activated/deactivated.""" - def __init__(self): + def __init__(self) -> None: self._enabled = False - def on_activate(self, state) -> TransitionCallbackReturn: + def on_activate(self, state: 'LifecycleState') -> TransitionCallbackReturn: self._enabled = True return TransitionCallbackReturn.SUCCESS - def on_deactivate(self, state) -> TransitionCallbackReturn: + def on_deactivate(self, state: 'LifecycleState') -> TransitionCallbackReturn: self._enabled = False return TransitionCallbackReturn.SUCCESS @property - def is_activated(self): + def is_activated(self) -> bool: return self._enabled @staticmethod diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 6528007d5..142f0dcf4 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -17,12 +17,13 @@ from typing import NamedTuple from typing import Optional from typing import Set +from typing import TYPE_CHECKING import lifecycle_msgs.msg import lifecycle_msgs.srv from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.service import Service @@ -31,8 +32,10 @@ from .managed_entity import ManagedEntity from .publisher import LifecyclePublisher +if TYPE_CHECKING: + from typing import TypeAlias -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType +TransitionCallbackReturn: 'TypeAlias' = _rclpy.TransitionCallbackReturnType class LifecycleState(NamedTuple): @@ -273,7 +276,7 @@ def on_error(self, state: LifecycleState) -> TransitionCallbackReturn: """ return self.__transition_callback_impl('on_error', state) - def create_lifecycle_publisher(self, *args, **kwargs): + def create_lifecycle_publisher(self, *args, **kwargs) -> LifecyclePublisher: # TODO(ivanpauno): Should we override lifecycle publisher? # There is an issue with python using the overridden method # when creating publishers for builitin publishers (like parameters events). @@ -337,7 +340,7 @@ def __change_state(self, transition_id: int) -> TransitionCallbackReturn: self._state_machine.trigger_transition_by_label(error_cb_ret_code.to_label(), True) return cb_return_code - def __check_is_initialized(self): + def __check_is_initialized(self) -> None: if not self._state_machine.initialized: raise RuntimeError( 'Internal error: got service request while lifecycle state machine ' @@ -347,7 +350,7 @@ def __on_change_state( self, req: lifecycle_msgs.srv.ChangeState.Request, resp: lifecycle_msgs.srv.ChangeState.Response - ): + ) -> lifecycle_msgs.srv.ChangeState.Response: self.__check_is_initialized() transition_id = req.transition.id if req.transition.label: @@ -364,7 +367,7 @@ def __on_get_state( self, req: lifecycle_msgs.srv.GetState.Request, resp: lifecycle_msgs.srv.GetState.Response - ): + ) -> lifecycle_msgs.srv.GetState.Response: self.__check_is_initialized() resp.current_state.id, resp.current_state.label = self._state_machine.current_state return resp @@ -373,7 +376,7 @@ def __on_get_available_states( self, req: lifecycle_msgs.srv.GetAvailableStates.Request, resp: lifecycle_msgs.srv.GetAvailableStates.Response - ): + ) -> lifecycle_msgs.srv.GetAvailableStates.Response: self.__check_is_initialized() for state_id, label in self._state_machine.available_states: resp.available_states.append(lifecycle_msgs.msg.State(id=state_id, label=label)) @@ -383,7 +386,7 @@ def __on_get_available_transitions( self, req: lifecycle_msgs.srv.GetAvailableTransitions.Request, resp: lifecycle_msgs.srv.GetAvailableTransitions.Response - ): + ) -> lifecycle_msgs.srv.GetAvailableTransitions.Response: self.__check_is_initialized() for transition_description in self._state_machine.available_transitions: transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ @@ -402,7 +405,7 @@ def __on_get_transition_graph( self, req: lifecycle_msgs.srv.GetAvailableTransitions.Request, resp: lifecycle_msgs.srv.GetAvailableTransitions.Response - ): + ) -> lifecycle_msgs.srv.GetAvailableTransitions.Response: self.__check_is_initialized() for transition_description in self._state_machine.transition_graph: transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ @@ -426,7 +429,7 @@ class LifecycleNode(LifecycleNodeMixin, Node): Methods in LifecycleNodeMixin override the ones in Node. """ - def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): + def __init__(self, node_name: str, *, enable_communication_interface: bool = True, **kwargs): """ Create a lifecycle node. diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 63999fab2..eff12cd45 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -20,7 +20,7 @@ from .managed_entity import SimpleManagedEntity -class LifecyclePublisher(SimpleManagedEntity, Publisher): +class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): """Managed publisher entity.""" def __init__(self, *args, **kwargs): From 82930c6ded187380276878ab73f0c54ddf0691c4 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 14:28:11 -0400 Subject: [PATCH 17/28] Add types to lifecylce objects Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 68 ++++++++++++- rclpy/rclpy/lifecycle/managed_entity.py | 25 ++++- rclpy/rclpy/lifecycle/node.py | 125 ++++++++++++++++++++---- rclpy/rclpy/lifecycle/publisher.py | 22 ++++- 4 files changed, 213 insertions(+), 27 deletions(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 75427df4d..46f527a8d 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -16,7 +16,7 @@ from __future__ import annotations from enum import IntEnum from types import TracebackType -from typing import Any, Generic, Literal, overload, Sequence, TypedDict +from typing import Any, Generic, Literal, overload, Sequence, TypeAlias, TypedDict from rclpy.clock import JumpHandle from rclpy.clock_type import ClockType @@ -135,6 +135,72 @@ class EventHandle(Destroyable): """Get pending data from a ready event.""" +LifecycleStateMachineState: TypeAlias = tuple[int, str] + + +class LifecycleStateMachine(Destroyable): + + def __init__(self, node: Node, enable_com_interface: bool) -> None: ... + + @property + def initialized(self) -> bool: + """Check if state machine is initialized.""" + + @property + def current_state(self) -> LifecycleStateMachineState: + """Get the current state machine state.""" + + @property + def available_states(self) -> list[LifecycleStateMachineState]: + """Get the available states.""" + + @property + def available_transitions(self) -> list[tuple[int, str, int, str, int, str]]: + """Get the available transitions.""" + + @property + def transition_graph(self) -> list[tuple[int, str, int, str, int, str]]: + """Get the transition graph.""" + + def get_transition_by_label(self, label: str) -> int: + """Get the transition id from a transition label.""" + + def trigger_transition_by_id(self, transition_id: int, publish_update: bool) -> None: + """Trigger a transition by transition id.""" + + def trigger_transition_by_label(self, label: str, publish_update: bool) -> None: + """Trigger a transition by label.""" + + @property + def service_change_state(self) -> Service: + """Get the change state service.""" + + @property + def service_get_state(self) -> Service: + """Get the get state service.""" + + @property + def service_get_available_states(self) -> Service: + """Get the get available states service.""" + + @property + def service_get_available_transitions(self) -> Service: + """Get the get available transitions service.""" + + @property + def service_get_transition_graph(self) -> Service: + """Get the get transition graph service.""" + + +class TransitionCallbackReturnType(IntEnum): + SUCCESS: int + FAILURE: int + ERROR: int + + def to_label(self) -> str: + """Convert the transition callback return code to a transition label.""" + + class GuardCondition(Destroyable): def __init__(self, context: Context) -> None: ... diff --git a/rclpy/rclpy/lifecycle/managed_entity.py b/rclpy/rclpy/lifecycle/managed_entity.py index 7115a91ac..a3de05a7a 100644 --- a/rclpy/rclpy/lifecycle/managed_entity.py +++ b/rclpy/rclpy/lifecycle/managed_entity.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import TYPE_CHECKING from functools import wraps +from typing import Any, Callable, Dict, List, Optional, overload, TYPE_CHECKING, Union from ..impl.implementation_singleton import rclpy_implementation as _rclpy @@ -71,10 +71,27 @@ def is_activated(self) -> bool: return self._enabled @staticmethod - def when_enabled(wrapped=None, *, when_not_enabled=None): - def decorator(wrapped): + @overload + def when_enabled(wrapped: None, *, + when_not_enabled: Optional[Callable[..., None]] = None + ) -> Callable[[Callable[..., None]], Callable[..., None]]: ... + + @staticmethod + @overload + def when_enabled(wrapped: Callable[..., None], *, + when_not_enabled: Optional[Callable[..., None]] = None + ) -> Callable[..., None]: ... + + @staticmethod + def when_enabled(wrapped: Optional[Callable[..., None]] = None, *, + when_not_enabled: Optional[Callable[..., None]] = None) -> Union[ + Callable[..., None], + Callable[[Callable[..., None]], Callable[..., None]] + ]: + def decorator(wrapped: Callable[..., None]) -> Callable[..., None]: @wraps(wrapped) - def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs): + def only_when_enabled_wrapper(self: SimpleManagedEntity, *args: List[Any], + **kwargs: Dict[str, Any]) -> None: if not self._enabled: if when_not_enabled is not None: when_not_enabled() diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 142f0dcf4..288393e70 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -12,32 +12,45 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import Any from typing import Callable from typing import Dict +from typing import List +from typing import Literal from typing import NamedTuple from typing import Optional from typing import Set +from typing import Type from typing import TYPE_CHECKING +from typing import Union import lifecycle_msgs.msg import lifecycle_msgs.srv from rclpy.callback_groups import CallbackGroup -from impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node from rclpy.qos import QoSProfile from rclpy.service import Service -from rclpy.type_support import check_is_valid_srv_type +from rclpy.type_support import check_is_valid_srv_type, MsgT from .managed_entity import ManagedEntity from .publisher import LifecyclePublisher if TYPE_CHECKING: from typing import TypeAlias + from rclpy.context import Context + from rclpy.parameter import Parameter + from rclpy.qos_overriding_options import QoSOverridingOptions + from rclpy.event_handler import PublisherEventCallbacks TransitionCallbackReturn: 'TypeAlias' = _rclpy.TransitionCallbackReturnType +CallbackNames = Literal['on_configure', 'on_cleanup', 'on_shutdown', 'on_activate', + 'on_deactivate', 'on_error'] + + class LifecycleState(NamedTuple): label: str state_id: int @@ -65,6 +78,10 @@ def __init__( :param callback_group: Callback group that will be used by all the lifecycle node services. """ + if not isinstance(self, Node): + raise RuntimeError('LifecycleNodeMixin uses Node fields so Node needs to be' + 'in the inheritance tree.') + self._callbacks: Dict[int, Callable[[LifecycleState], TransitionCallbackReturn]] = {} # register all state machine transition callbacks self.__register_callback( @@ -150,13 +167,13 @@ def __init__( # Extend base class list of services, so they are added to the executor when spinning. self._services.extend(lifecycle_services) - def trigger_configure(self): + def trigger_configure(self) -> TransitionCallbackReturn: return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE) - def trigger_cleanup(self): + def trigger_cleanup(self) -> TransitionCallbackReturn: return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP) - def trigger_shutdown(self): + def trigger_shutdown(self) -> TransitionCallbackReturn: current_state = self._state_machine.current_state[1] if current_state == 'unconfigured': return self.__change_state( @@ -168,22 +185,37 @@ def trigger_shutdown(self): return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN) raise _rclpy.RCLError('Shutdown transtion not possible') - def trigger_activate(self): + def trigger_activate(self) -> TransitionCallbackReturn: return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE) - def trigger_deactivate(self): + def trigger_deactivate(self) -> TransitionCallbackReturn: return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE) - def add_managed_entity(self, entity: ManagedEntity): + def add_managed_entity(self, entity: ManagedEntity) -> None: if not isinstance(entity, ManagedEntity): raise TypeError('Expected a rclpy.lifecycle.ManagedEntity instance.') self._managed_entities.add(entity) - def __transition_callback_impl(self, callback_name: str, state: LifecycleState): + def __transition_callback_impl(self, callback_name: CallbackNames, + state: LifecycleState) -> TransitionCallbackReturn: for entity in self._managed_entities: - cb = getattr(entity, callback_name) + if callback_name == 'on_activate': + cb = entity.on_activate + elif callback_name == 'on_cleanup': + cb = entity.on_cleanup + elif callback_name == 'on_configure': + cb = entity.on_configure + elif callback_name == 'on_deactivate': + cb = entity.on_deactivate + elif callback_name == 'on_error': + cb = entity.on_error + elif callback_name == 'on_shutdown': + cb = entity.on_shutdown + else: + raise ValueError(f'Not valid callback name "{callback_name}" given.') + ret = cb(state) - if not isinstance(ret, TransitionCallbackReturn): + if not isinstance(ret, _rclpy.TransitionCallbackReturnType): raise TypeError( f'{callback_name}() return value of class {type(entity)} should be' ' `TransitionCallbackReturn`.\n' @@ -276,30 +308,56 @@ def on_error(self, state: LifecycleState) -> TransitionCallbackReturn: """ return self.__transition_callback_impl('on_error', state) - def create_lifecycle_publisher(self, *args, **kwargs) -> LifecyclePublisher: + def create_lifecycle_publisher( + self, + msg_type: Type[MsgT], + topic: str, + qos_profile: Union[QoSProfile, int], + *, + callback_group: Optional[CallbackGroup] = None, + event_callbacks: Optional[PublisherEventCallbacks] = None, + qos_overriding_options: Optional[QoSOverridingOptions] = None, + publisher_class: None = None + ) -> LifecyclePublisher[MsgT]: # TODO(ivanpauno): Should we override lifecycle publisher? # There is an issue with python using the overridden method # when creating publishers for builitin publishers (like parameters events). # We could override them after init, similar to what we do to override publish() # in LifecycleNode. # Having both options seem fine. - if 'publisher_class' in kwargs: + if not isinstance(self, Node): + raise RuntimeError('LifecycleNodeMixin uses Node fields so Node needs to be' + 'in the inheritance tree.') + + if publisher_class: raise TypeError( "create_publisher() got an unexpected keyword argument 'publisher_class'") - pub = Node.create_publisher(self, *args, **kwargs, publisher_class=LifecyclePublisher) + pub = Node.create_publisher(self, msg_type, topic, qos_profile, + callback_group=callback_group, + event_callbacks=event_callbacks, + qos_overriding_options=qos_overriding_options, + publisher_class=LifecyclePublisher) + + if not isinstance(pub, LifecyclePublisher): + raise Exception('Node failed to create LifecyclePublisher.') + self._managed_entities.add(pub) return pub - def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher): + def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher[Any]) -> bool: + if not isinstance(self, Node): + raise RuntimeError('LifecycleNodeMixin uses Node fields so Node needs to be' + 'in the inheritance tree.') + try: self._managed_entities.remove(publisher) except KeyError: pass - return Node.destroy_publisher(self, publisher) + return self.destroy_publisher(publisher) def __register_callback( self, state_id: int, callback: Callable[[LifecycleState], TransitionCallbackReturn] - ) -> bool: + ) -> Literal[True]: """ Register a callback that will be triggered when transitioning to state_id. @@ -429,14 +487,43 @@ class LifecycleNode(LifecycleNodeMixin, Node): Methods in LifecycleNodeMixin override the ones in Node. """ - def __init__(self, node_name: str, *, enable_communication_interface: bool = True, **kwargs): + def __init__( + self, + node_name: str, + *, + context: Optional[Context], + cli_args: Optional[List[str]], + namespace: Optional[str], + use_global_arguments: bool, + enable_rosout: bool, + start_parameter_services: bool, + parameter_overrides: Optional[List[Parameter]], + allow_undeclared_parameters: bool, + automatically_declare_parameters_from_overrides: bool, + enable_logger_service: bool, + enable_communication_interface: bool = True, + ) -> None: """ Create a lifecycle node. See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node() for the documentation of each parameter. """ - Node.__init__(self, node_name, **kwargs) + Node.__init__( + self, + node_name, + context=context, + cli_args=cli_args, + namespace=namespace, + use_global_arguments=use_global_arguments, + enable_rosout=enable_rosout, + start_parameter_services=start_parameter_services, + parameter_overrides=parameter_overrides, + allow_undeclared_parameters=allow_undeclared_parameters, + automatically_declare_parameters_from_overrides=( + automatically_declare_parameters_from_overrides + ), + enable_logger_service=enable_logger_service) LifecycleNodeMixin.__init__( self, enable_communication_interface=enable_communication_interface) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index eff12cd45..2c8f3d787 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Union +from typing import Type, TYPE_CHECKING, Union from rclpy.publisher import Publisher from rclpy.type_support import MsgT @@ -20,12 +20,28 @@ from .managed_entity import SimpleManagedEntity +if TYPE_CHECKING: + from rclpy.qos import QoSProfile + from rclpy.callback_groups import CallbackGroup + from rclpy.event_handler import PublisherEventCallbacks + from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy + + class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): """Managed publisher entity.""" - def __init__(self, *args, **kwargs): + def __init__( + self, + publisher_impl: '_rclpy.Publisher[MsgT]', + msg_type: Type[MsgT], + topic: str, + qos_profile: QoSProfile, + event_callbacks: PublisherEventCallbacks, + callback_group: CallbackGroup + ) -> None: SimpleManagedEntity.__init__(self) - Publisher.__init__(self, *args, **kwargs) + Publisher.__init__(self, publisher_impl, msg_type, topic, qos_profile, event_callbacks, + callback_group) @SimpleManagedEntity.when_enabled def publish(self, msg: Union[MsgT, bytes]) -> None: From ad431f2809e9fbd66258c1d9a4204aeb02364a63 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 14:38:12 -0400 Subject: [PATCH 18/28] move types into string literals Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 8 ++++---- rclpy/rclpy/lifecycle/publisher.py | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 288393e70..ad398911a 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -315,8 +315,8 @@ def create_lifecycle_publisher( qos_profile: Union[QoSProfile, int], *, callback_group: Optional[CallbackGroup] = None, - event_callbacks: Optional[PublisherEventCallbacks] = None, - qos_overriding_options: Optional[QoSOverridingOptions] = None, + event_callbacks: 'Optional[PublisherEventCallbacks]' = None, + qos_overriding_options: 'Optional[QoSOverridingOptions]' = None, publisher_class: None = None ) -> LifecyclePublisher[MsgT]: # TODO(ivanpauno): Should we override lifecycle publisher? @@ -491,13 +491,13 @@ def __init__( self, node_name: str, *, - context: Optional[Context], + context: 'Optional[Context]', cli_args: Optional[List[str]], namespace: Optional[str], use_global_arguments: bool, enable_rosout: bool, start_parameter_services: bool, - parameter_overrides: Optional[List[Parameter]], + parameter_overrides: 'Optional[List[Parameter]]', allow_undeclared_parameters: bool, automatically_declare_parameters_from_overrides: bool, enable_logger_service: bool, diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 2c8f3d787..936079d8e 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -35,9 +35,9 @@ def __init__( publisher_impl: '_rclpy.Publisher[MsgT]', msg_type: Type[MsgT], topic: str, - qos_profile: QoSProfile, - event_callbacks: PublisherEventCallbacks, - callback_group: CallbackGroup + qos_profile: 'QoSProfile', + event_callbacks: 'PublisherEventCallbacks', + callback_group: 'CallbackGroup' ) -> None: SimpleManagedEntity.__init__(self) Publisher.__init__(self, publisher_impl, msg_type, topic, qos_profile, event_callbacks, From 9b55cec509cdc5a5acc057ec7667c938d8f97d16 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 18:34:12 -0400 Subject: [PATCH 19/28] use unpack instead Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 41 +++++++++++++----------------- rclpy/rclpy/lifecycle/publisher.py | 23 ++++++++++------- 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index ad398911a..b9ee432b6 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -22,8 +22,8 @@ from typing import Set from typing import Type from typing import TYPE_CHECKING +from typing import TypedDict from typing import Union - import lifecycle_msgs.msg import lifecycle_msgs.srv @@ -39,6 +39,8 @@ if TYPE_CHECKING: from typing import TypeAlias + from typing import Unpack + from rclpy.context import Context from rclpy.parameter import Parameter from rclpy.qos_overriding_options import QoSOverridingOptions @@ -479,6 +481,19 @@ def __on_get_transition_graph( return resp +class LifecycleNodeArgs(TypedDict): + context: Optional[Context] + cli_args: Optional[List[str]] + namespace: Optional[str] + use_global_arguments: bool + enable_rosout: bool + start_parameter_services: bool + parameter_overrides: 'Optional[List[Parameter]]' + allow_undeclared_parameters: bool + automatically_declare_parameters_from_overrides: bool + enable_logger_service: bool + + class LifecycleNode(LifecycleNodeMixin, Node): """ A ROS 2 managed node. @@ -491,17 +506,8 @@ def __init__( self, node_name: str, *, - context: 'Optional[Context]', - cli_args: Optional[List[str]], - namespace: Optional[str], - use_global_arguments: bool, - enable_rosout: bool, - start_parameter_services: bool, - parameter_overrides: 'Optional[List[Parameter]]', - allow_undeclared_parameters: bool, - automatically_declare_parameters_from_overrides: bool, - enable_logger_service: bool, enable_communication_interface: bool = True, + **kwargs: 'Unpack[LifecycleNodeArgs]', ) -> None: """ Create a lifecycle node. @@ -512,18 +518,7 @@ def __init__( Node.__init__( self, node_name, - context=context, - cli_args=cli_args, - namespace=namespace, - use_global_arguments=use_global_arguments, - enable_rosout=enable_rosout, - start_parameter_services=start_parameter_services, - parameter_overrides=parameter_overrides, - allow_undeclared_parameters=allow_undeclared_parameters, - automatically_declare_parameters_from_overrides=( - automatically_declare_parameters_from_overrides - ), - enable_logger_service=enable_logger_service) + **kwargs) LifecycleNodeMixin.__init__( self, enable_communication_interface=enable_communication_interface) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 936079d8e..4f641d5cd 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Type, TYPE_CHECKING, Union +from typing import Type, TYPE_CHECKING, TypedDict, Union, Generic from rclpy.publisher import Publisher from rclpy.type_support import MsgT @@ -21,27 +21,32 @@ if TYPE_CHECKING: + from typing import Unpack + from rclpy.qos import QoSProfile from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import PublisherEventCallbacks from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +class LifecyclePublisherArgs(TypedDict, Generic[MsgT]): + publisher_impl: '_rclpy.Publisher[MsgT]' + msg_type: Type[MsgT] + topic: str + qos_profile: 'QoSProfile' + event_callbacks: 'PublisherEventCallbacks' + callback_group: 'CallbackGroup' + + class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): """Managed publisher entity.""" def __init__( self, - publisher_impl: '_rclpy.Publisher[MsgT]', - msg_type: Type[MsgT], - topic: str, - qos_profile: 'QoSProfile', - event_callbacks: 'PublisherEventCallbacks', - callback_group: 'CallbackGroup' + **kwargs: 'Unpack[LifecyclePublisherArgs]' ) -> None: SimpleManagedEntity.__init__(self) - Publisher.__init__(self, publisher_impl, msg_type, topic, qos_profile, event_callbacks, - callback_group) + Publisher.__init__(self, **kwargs) @SimpleManagedEntity.when_enabled def publish(self, msg: Union[MsgT, bytes]) -> None: From 79ad27634387e783df29f3e5fb18895f2ac3b2e7 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 18:41:43 -0400 Subject: [PATCH 20/28] flake8 Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 1 + rclpy/rclpy/lifecycle/publisher.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index b9ee432b6..2726b0009 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -24,6 +24,7 @@ from typing import TYPE_CHECKING from typing import TypedDict from typing import Union + import lifecycle_msgs.msg import lifecycle_msgs.srv diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 4f641d5cd..e103133b1 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Type, TYPE_CHECKING, TypedDict, Union, Generic +from typing import Generic, Type, TYPE_CHECKING, TypedDict, Union from rclpy.publisher import Publisher from rclpy.type_support import MsgT From 31f6b9b5425ee1e53cdff0a095c50f7922d22e92 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 18:42:58 -0400 Subject: [PATCH 21/28] move Context into string Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 2726b0009..fc6b97518 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -483,7 +483,7 @@ def __on_get_transition_graph( class LifecycleNodeArgs(TypedDict): - context: Optional[Context] + context: 'Optional[Context]' cli_args: Optional[List[str]] namespace: Optional[str] use_global_arguments: bool From 31a7734d4e638d2612bd743c264f30f20861df8a Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 18:52:51 -0400 Subject: [PATCH 22/28] update create_lifecycle_publisher Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 18 ++++++++++-------- rclpy/rclpy/lifecycle/publisher.py | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index fc6b97518..5187c2a11 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -59,6 +59,12 @@ class LifecycleState(NamedTuple): state_id: int +class CreateLifecyclePublisherArgs(TypedDict): + callback_group: Optional[CallbackGroup] + event_callbacks: 'Optional[PublisherEventCallbacks]' + qos_overriding_options: 'Optional[QoSOverridingOptions]' + + class LifecycleNodeMixin(ManagedEntity): """ Mixin class to share as most code as possible between `Node` and `LifecycleNode`. @@ -317,10 +323,8 @@ def create_lifecycle_publisher( topic: str, qos_profile: Union[QoSProfile, int], *, - callback_group: Optional[CallbackGroup] = None, - event_callbacks: 'Optional[PublisherEventCallbacks]' = None, - qos_overriding_options: 'Optional[QoSOverridingOptions]' = None, - publisher_class: None = None + publisher_class: None = None, + **kwargs: 'Unpack[CreateLifecyclePublisherArgs]' ) -> LifecyclePublisher[MsgT]: # TODO(ivanpauno): Should we override lifecycle publisher? # There is an issue with python using the overridden method @@ -336,10 +340,8 @@ def create_lifecycle_publisher( raise TypeError( "create_publisher() got an unexpected keyword argument 'publisher_class'") pub = Node.create_publisher(self, msg_type, topic, qos_profile, - callback_group=callback_group, - event_callbacks=event_callbacks, - qos_overriding_options=qos_overriding_options, - publisher_class=LifecyclePublisher) + publisher_class=LifecyclePublisher, + **kwargs) if not isinstance(pub, LifecyclePublisher): raise Exception('Node failed to create LifecyclePublisher.') diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index e103133b1..a12d628dd 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -43,7 +43,7 @@ class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): def __init__( self, - **kwargs: 'Unpack[LifecyclePublisherArgs]' + **kwargs: 'Unpack[LifecyclePublisherArgs[MsgT]]' ) -> None: SimpleManagedEntity.__init__(self) Publisher.__init__(self, **kwargs) From 94098d8b9100daf3f57b6dccf453d8ba1365757e Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Thu, 22 Aug 2024 19:24:46 -0400 Subject: [PATCH 23/28] test Unpack tuple Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/publisher.py | 34 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index a12d628dd..aa04571b7 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -12,30 +12,28 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Generic, Type, TYPE_CHECKING, TypedDict, Union +from __future__ import annotations + +from typing import Generic, Tuple, Type, TypedDict, Union, Unpack + +from rclpy.callback_groups import CallbackGroup +from rclpy.event_handler import PublisherEventCallbacks +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.publisher import Publisher +from rclpy.qos import QoSProfile from rclpy.type_support import MsgT from .managed_entity import SimpleManagedEntity -if TYPE_CHECKING: - from typing import Unpack - - from rclpy.qos import QoSProfile - from rclpy.callback_groups import CallbackGroup - from rclpy.event_handler import PublisherEventCallbacks - from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -class LifecyclePublisherArgs(TypedDict, Generic[MsgT]): - publisher_impl: '_rclpy.Publisher[MsgT]' +class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): + publisher_impl: _rclpy.Publisher[MsgT] msg_type: Type[MsgT] topic: str - qos_profile: 'QoSProfile' - event_callbacks: 'PublisherEventCallbacks' - callback_group: 'CallbackGroup' + qos_profile: QoSProfile + event_callbacks: PublisherEventCallbacks + callback_group: CallbackGroup class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): @@ -43,10 +41,12 @@ class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): def __init__( self, - **kwargs: 'Unpack[LifecyclePublisherArgs[MsgT]]' + *args: Unpack[Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile, + PublisherEventCallbacks, CallbackGroup]], + **kwargs: Unpack[LifecyclePublisherKWArgs[MsgT]] ) -> None: SimpleManagedEntity.__init__(self) - Publisher.__init__(self, **kwargs) + Publisher.__init__(self, *args, **kwargs) @SimpleManagedEntity.when_enabled def publish(self, msg: Union[MsgT, bytes]) -> None: From 30d22fe8431c645f2dc4007ba3d646790952b3ed Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 23 Aug 2024 13:20:42 -0400 Subject: [PATCH 24/28] use Enum Signed-off-by: Michael Carlstrom --- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 39 +++++++++++++++------------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 46f527a8d..56e1f1446 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -14,7 +14,7 @@ from __future__ import annotations -from enum import IntEnum +from enum import Enum from types import TracebackType from typing import Any, Generic, Literal, overload, Sequence, TypeAlias, TypedDict @@ -101,21 +101,23 @@ class rcl_duration_t: nanoseconds: int -class rcl_subscription_event_type_t(IntEnum): - RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED: int - RCL_SUBSCRIPTION_LIVELINESS_CHANGED: int - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS: int - RCL_SUBSCRIPTION_MESSAGE_LOST: int - RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE: int - RCL_SUBSCRIPTION_MATCHED: int +class rcl_subscription_event_type_t(Enum): + _value_: int + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED = ... + RCL_SUBSCRIPTION_LIVELINESS_CHANGED = ... + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS = ... + RCL_SUBSCRIPTION_MESSAGE_LOST = ... + RCL_SUBSCRIPTION_INCOMPATIBLE_TYPE = ... + RCL_SUBSCRIPTION_MATCHED = ... -class rcl_publisher_event_type_t(IntEnum): - RCL_PUBLISHER_OFFERED_DEADLINE_MISSED: int - RCL_PUBLISHER_LIVELINESS_LOST: int - RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS: int - RCL_PUBLISHER_INCOMPATIBLE_TYPE: int - RCL_PUBLISHER_MATCHED: int +class rcl_publisher_event_type_t(Enum): + _value_: int + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED = ... + RCL_PUBLISHER_LIVELINESS_LOST = ... + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS = ... + RCL_PUBLISHER_INCOMPATIBLE_TYPE = ... + RCL_PUBLISHER_MATCHED = ... class EventHandle(Destroyable): @@ -192,10 +194,11 @@ class LifecycleStateMachine(Destroyable): """Get the get transition graph service.""" -class TransitionCallbackReturnType(IntEnum): - SUCCESS: int - FAILURE: int - ERROR: int +class TransitionCallbackReturnType(Enum): + _value_: int + SUCCESS = ... + FAILURE = ... + ERROR = ... def to_label(self) -> str: """Convert the transition callback return code to a transition label.""" From d0e2786c7cd4b2de8560127a2bda3830191e73a7 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 23 Aug 2024 14:16:50 -0400 Subject: [PATCH 25/28] use RuntimeError Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/rclpy/lifecycle/node.py b/rclpy/rclpy/lifecycle/node.py index 5187c2a11..7fd424e07 100644 --- a/rclpy/rclpy/lifecycle/node.py +++ b/rclpy/rclpy/lifecycle/node.py @@ -344,7 +344,7 @@ def create_lifecycle_publisher( **kwargs) if not isinstance(pub, LifecyclePublisher): - raise Exception('Node failed to create LifecyclePublisher.') + raise RuntimeError('Node failed to create LifecyclePublisher.') self._managed_entities.add(pub) return pub From b2dca14edb01668ab9fe1474d026a87ab6c20615 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Mon, 26 Aug 2024 17:28:36 -0400 Subject: [PATCH 26/28] Mpve Unpack import Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/publisher.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index aa04571b7..da67fffb8 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -15,7 +15,7 @@ from __future__ import annotations -from typing import Generic, Tuple, Type, TypedDict, Union, Unpack +from typing import Generic, Tuple, Type, TYPE_CHECKING, TypedDict, Union from rclpy.callback_groups import CallbackGroup from rclpy.event_handler import PublisherEventCallbacks @@ -26,6 +26,11 @@ from .managed_entity import SimpleManagedEntity +if TYPE_CHECKING: + from typing import TypeAlias, Unpack + LifecyclePublisherArgs: TypeAlias = Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile, + PublisherEventCallbacks, CallbackGroup] + class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): publisher_impl: _rclpy.Publisher[MsgT] @@ -41,9 +46,8 @@ class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): def __init__( self, - *args: Unpack[Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile, - PublisherEventCallbacks, CallbackGroup]], - **kwargs: Unpack[LifecyclePublisherKWArgs[MsgT]] + *args: 'Unpack[LifecyclePublisherArgs]', + **kwargs: 'Unpack[LifecyclePublisherKWArgs[MsgT]]' ) -> None: SimpleManagedEntity.__init__(self) Publisher.__init__(self, *args, **kwargs) From f6316f924215206123979fdc8aa418fce792fff7 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Tue, 27 Aug 2024 07:10:59 -0400 Subject: [PATCH 27/28] move type inside TYPE_CHECKING block Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/publisher.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index da67fffb8..53ecd391f 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -14,7 +14,7 @@ from __future__ import annotations - +import sys from typing import Generic, Tuple, Type, TYPE_CHECKING, TypedDict, Union from rclpy.callback_groups import CallbackGroup @@ -32,13 +32,13 @@ PublisherEventCallbacks, CallbackGroup] -class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): - publisher_impl: _rclpy.Publisher[MsgT] - msg_type: Type[MsgT] - topic: str - qos_profile: QoSProfile - event_callbacks: PublisherEventCallbacks - callback_group: CallbackGroup + class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): + publisher_impl: _rclpy.Publisher[MsgT] + msg_type: Type[MsgT] + topic: str + qos_profile: QoSProfile + event_callbacks: PublisherEventCallbacks + callback_group: CallbackGroup class LifecyclePublisher(SimpleManagedEntity, Publisher[MsgT]): From 8efb1a1c34003ba4cad314ae7b99324234edaa76 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Tue, 27 Aug 2024 09:45:36 -0400 Subject: [PATCH 28/28] flake8 Signed-off-by: Michael Carlstrom --- rclpy/rclpy/lifecycle/publisher.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclpy/rclpy/lifecycle/publisher.py b/rclpy/rclpy/lifecycle/publisher.py index 53ecd391f..af6b33572 100644 --- a/rclpy/rclpy/lifecycle/publisher.py +++ b/rclpy/rclpy/lifecycle/publisher.py @@ -14,7 +14,6 @@ from __future__ import annotations -import sys from typing import Generic, Tuple, Type, TYPE_CHECKING, TypedDict, Union from rclpy.callback_groups import CallbackGroup @@ -31,7 +30,6 @@ LifecyclePublisherArgs: TypeAlias = Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile, PublisherEventCallbacks, CallbackGroup] - class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]): publisher_impl: _rclpy.Publisher[MsgT] msg_type: Type[MsgT]