From f338416b76049e79ada9aa00e41ea52137e35029 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom Date: Fri, 23 Aug 2024 12:22:50 -0400 Subject: [PATCH] Add types to wait_for_message.py and moves Handles into type stubs (#1325) * Add types to wait_for_message.py Signed-off-by: Michael Carlstrom * Add copyright Signed-off-by: Michael Carlstrom * re-run CI Signed-off-by: Michael Carlstrom * re-run CI Signed-off-by: Michael Carlstrom * move Handles into _rclpy_pybind11 Signed-off-by: Michael Carlstrom * Move Handles into type stubs: Signed-off-by: Michael Carlstrom * Move Handles into type stubs Signed-off-by: Michael Carlstrom * move [] into string Signed-off-by: Michael Carlstrom * fix imports Signed-off-by: Michael Carlstrom * remove extra line Signed-off-by: Michael Carlstrom * puy _rclpy.Publisher in quotes Signed-off-by: Michael Carlstrom * fix capitalization Signed-off-by: Michael Carlstrom * Add EventHandle Constructor Signed-off-by: Michael Carlstrom * Use RuntimeError for context Signed-off-by: Michael Carlstrom * Add TYPE_CHECKING import Signed-off-by: Michael Carlstrom --------- Signed-off-by: Michael Carlstrom Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> --- 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 | 6 +- rclpy/rclpy/guard_condition.py | 13 +- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 323 ++++++++++++++++++- rclpy/rclpy/impl/implementation_singleton.py | 1 + rclpy/rclpy/node.py | 7 +- rclpy/rclpy/publisher.py | 40 +-- rclpy/rclpy/qos.py | 18 +- rclpy/rclpy/subscription.py | 34 +- rclpy/rclpy/time.py | 23 +- rclpy/rclpy/timer.py | 39 +-- rclpy/rclpy/wait_for_message.py | 13 +- 15 files changed, 366 insertions(+), 250 deletions(-) delete mode 100644 rclpy/rclpy/destroyable.py 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 427313c4b..e846b9135 100644 --- a/rclpy/rclpy/event_handler.py +++ b/rclpy/rclpy/event_handler.py @@ -32,10 +32,6 @@ from typing import TypeAlias -if TYPE_CHECKING: - from rclpy.subscription import SubscriptionHandle - - QoSPublisherEventType = _rclpy.rcl_publisher_event_type_t QoSSubscriptionEventType = _rclpy.rcl_subscription_event_type_t @@ -201,7 +197,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 fb36923f6..416cf1829 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -12,21 +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 -from rclpy.destroyable import DestroyableType from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context -class GuardConditionHandle(DestroyableType, Protocol): - - def trigger_guard_condition(self) -> None: - ... - - class GuardCondition: def __init__(self, callback: Optional[Callable], @@ -44,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 @@ -57,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/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index df259e477..75427df4d 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -12,11 +12,324 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Sequence +from __future__ import annotations +from enum import IntEnum +from types import TracebackType +from typing import Any, Generic, Literal, overload, 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 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): + + 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]): + + 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.""" + + 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: rmw_qos_profile_t) -> 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 ba7f55fe8..3e312280b 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -27,6 +27,7 @@ """ from rpyutils import import_c_library + package = 'rclpy' rclpy_implementation = import_c_library('._rclpy_pybind11', package) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index e51dbcd5d..cba822bbb 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -79,7 +79,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 @@ -114,10 +113,6 @@ NodeNameNonExistentError = _rclpy.NodeNameNonExistentError -class NodeHandle: - pass - - class Node: """ A Node in the ROS graph. @@ -1658,7 +1653,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..66797f2c4 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/subscription.py b/rclpy/rclpy/subscription.py index 564742d86..86eedb0a9 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,30 +32,6 @@ 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') @@ -68,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]], @@ -114,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: @@ -157,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 7e1b1feb9..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. @@ -130,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 @@ -141,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_for_message.py b/rclpy/rclpy/wait_for_message.py index a26eb7516..82302fff9 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 RuntimeError('Cannot create Waitset without a context.handle') + wait_set = _rclpy.WaitSet(1, 1, 0, 0, 0, 0, context.handle) wait_set.clear_entities()