From 2de5c8ea447a1614a7c36d0fcf2250cefc21a594 Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Thu, 19 Sep 2024 17:23:04 -0700 Subject: [PATCH] Revert "Add types to Action Server and Action Client (#1349)" (#1359) This reverts commit d1d7d0540ca9769e98dd11cf5d52622d9e0b2ace. Signed-off-by: Tomoya Fujita --- rclpy/rclpy/action/client.py | 222 ++++++------------ rclpy/rclpy/action/server.py | 221 ++++++----------- rclpy/rclpy/client.py | 8 +- rclpy/rclpy/impl/_rclpy_pybind11.pyi | 194 +-------------- rclpy/rclpy/impl/implementation_singleton.pyi | 2 +- rclpy/rclpy/node.py | 21 +- rclpy/rclpy/service.py | 8 +- rclpy/rclpy/type_support.py | 96 +------- rclpy/rclpy/waitable.py | 4 +- rclpy/src/rclpy/action_server.cpp | 2 +- 10 files changed, 189 insertions(+), 589 deletions(-) diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 7a2587361..f22cca0d1 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -14,112 +14,72 @@ import threading import time -from types import TracebackType from typing import Any -from typing import Callable -from typing import Dict -from typing import Generic -from typing import Optional -from typing import Tuple -from typing import Type -from typing import TYPE_CHECKING from typing import TypedDict -from typing import TypeVar import uuid import weakref from action_msgs.msg import GoalStatus -from action_msgs.msg._goal_status_array import GoalStatusArray from action_msgs.srv import CancelGoal -from builtin_interfaces.msg import Time + from rclpy.executors import await_or_execute from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import qos_profile_action_status_default from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile from rclpy.task import Future -from rclpy.type_support import Action from rclpy.type_support import check_for_type_support -from rclpy.type_support import FeedbackMessage -from rclpy.type_support import FeedbackT -from rclpy.type_support import GetResultServiceResponse -from rclpy.type_support import GoalT -from rclpy.type_support import ResultT -from rclpy.type_support import SendGoalServiceResponse from rclpy.waitable import NumberOfEntities, Waitable -from unique_identifier_msgs.msg import UUID - -if TYPE_CHECKING: - from rclpy.node import Node - from rclpy.callback_groups import CallbackGroup - from typing_extensions import Unpack, TypeAlias - - ClientGoalHandleDictResultT = TypeVar('ClientGoalHandleDictResultT') - ClientGoalHandleDictFeedbackT = TypeVar('ClientGoalHandleDictFeedbackT') - - class ClientGoalHandleDict(TypedDict, - Generic[ClientGoalHandleDictResultT, ClientGoalHandleDictFeedbackT], - total=False): - goal: Tuple[int, SendGoalServiceResponse] - cancel: Tuple[int, CancelGoal.Response] - result: Tuple[int, GetResultServiceResponse[ClientGoalHandleDictResultT]] - feedback: FeedbackMessage[ClientGoalHandleDictFeedbackT] - status: GoalStatusArray -else: - ClientGoalHandleDict: 'TypeAlias' = Dict[str, object] - -T = TypeVar('T') +from unique_identifier_msgs.msg import UUID -class SendGoalKWargs(TypedDict): - feedback_callback: Optional[Callable[[FeedbackT], None]] - goal_uuid: Optional[UUID] +class ClientGoalHandleDict(TypedDict, total=False): + goal: Any + cancel: Any + result: Any + feedback: Any + status: Any -class ClientGoalHandle(Generic[GoalT, ResultT, FeedbackT]): +class ClientGoalHandle(): """Goal handle for working with Action Clients.""" - def __init__(self, action_client: 'ActionClient[GoalT, ResultT, FeedbackT]', - goal_id: UUID, goal_response: SendGoalServiceResponse): + def __init__(self, action_client, goal_id, goal_response): self._action_client = action_client self._goal_id = goal_id self._goal_response = goal_response self._status = GoalStatus.STATUS_UNKNOWN - def __eq__(self, other: object) -> bool: - if not isinstance(other, ClientGoalHandle): - return False + def __eq__(self, other): return self._goal_id == other.goal_id - def __ne__(self, other: object) -> bool: - if not isinstance(other, ClientGoalHandle): - return True + def __ne__(self, other): return self._goal_id != other.goal_id - def __repr__(self) -> str: + def __repr__(self): return 'ClientGoalHandle '.format( self.goal_id.uuid, self.accepted, self.status) @property - def goal_id(self) -> UUID: + def goal_id(self): return self._goal_id @property - def stamp(self) -> Time: + def stamp(self): return self._goal_response.stamp @property - def accepted(self) -> bool: + def accepted(self): return self._goal_response.accepted @property - def status(self) -> int: + def status(self): return self._status - def cancel_goal(self) -> Optional[CancelGoal.Response]: + def cancel_goal(self): """ Send a cancel request for the goal and wait for the response. @@ -129,7 +89,7 @@ def cancel_goal(self) -> Optional[CancelGoal.Response]: """ return self._action_client._cancel_goal(self) - def cancel_goal_async(self) -> Future[CancelGoal.Response]: + def cancel_goal_async(self): """ Asynchronous request for the goal be canceled. @@ -138,7 +98,7 @@ def cancel_goal_async(self) -> Future[CancelGoal.Response]: """ return self._action_client._cancel_goal_async(self) - def get_result(self) -> Optional[GetResultServiceResponse[ResultT]]: + def get_result(self): """ Request the result for the goal and wait for the response. @@ -148,7 +108,7 @@ def get_result(self) -> Optional[GetResultServiceResponse[ResultT]]: """ return self._action_client._get_result(self) - def get_result_async(self) -> Future[GetResultServiceResponse[ResultT]]: + def get_result_async(self): """ Asynchronously request the goal result. @@ -158,23 +118,22 @@ def get_result_async(self) -> Future[GetResultServiceResponse[ResultT]]: return self._action_client._get_result_async(self) -class ActionClient(Generic[GoalT, ResultT, FeedbackT], - Waitable['ClientGoalHandleDict[ResultT, FeedbackT]']): +class ActionClient(Waitable[ClientGoalHandleDict]): """ROS Action client.""" def __init__( self, - node: 'Node', - action_type: Type[Action[GoalT, ResultT, FeedbackT]], - action_name: str, + node, + action_type, + action_name, *, - callback_group: 'Optional[CallbackGroup]' = None, - goal_service_qos_profile: QoSProfile = qos_profile_services_default, - result_service_qos_profile: QoSProfile = qos_profile_services_default, - cancel_service_qos_profile: QoSProfile = qos_profile_services_default, - feedback_sub_qos_profile: QoSProfile = QoSProfile(depth=10), - status_sub_qos_profile: QoSProfile = qos_profile_action_status_default - ) -> None: + callback_group=None, + goal_service_qos_profile=qos_profile_services_default, + result_service_qos_profile=qos_profile_services_default, + cancel_service_qos_profile=qos_profile_services_default, + feedback_sub_qos_profile=QoSProfile(depth=10), + status_sub_qos_profile=qos_profile_action_status_default + ): """ Create an ActionClient. @@ -215,34 +174,28 @@ def __init__( self._is_ready = False # key: UUID in bytes, value: weak reference to ClientGoalHandle - self._goal_handles: Dict[bytes, - weakref.ReferenceType[ClientGoalHandle[GoalT, - ResultT, - FeedbackT]]] = {} + self._goal_handles = {} # key: goal request sequence_number, value: Future for goal response - self._pending_goal_requests: Dict[int, Future[ClientGoalHandle[GoalT, - ResultT, - FeedbackT]]] = {} + self._pending_goal_requests = {} # key: goal request sequence_number, value: UUID - self._goal_sequence_number_to_goal_id: Dict[int, UUID] = {} + self._goal_sequence_number_to_goal_id = {} # key: cancel request sequence number, value: Future for cancel response - self._pending_cancel_requests: Dict[int, Future[CancelGoal.Response]] = {} + self._pending_cancel_requests = {} # key: result request sequence number, value: Future for result response - self._pending_result_requests: Dict[int, Future[GetResultServiceResponse[ResultT]]] = {} + self._pending_result_requests = {} # key: result request sequence_number, value: UUID - self._result_sequence_number_to_goal_id: Dict[int, UUID] = {} + self._result_sequence_number_to_goal_id = {} # key: UUID in bytes, value: callback function - self._feedback_callbacks: Dict[bytes, Callable[[FeedbackT], None]] = {} + self._feedback_callbacks = {} callback_group.add_entity(self) self._node.add_waitable(self) self._logger = self._node.get_logger().get_child('action_client') - def _generate_random_uuid(self) -> UUID: + def _generate_random_uuid(self): return UUID(uuid=list(uuid.uuid4().bytes)) - def _remove_pending_request(self, future: Future[T], pending_requests: Dict[int, Future[T]] - ) -> Optional[int]: + def _remove_pending_request(self, future, pending_requests): """ Remove a future from the list of pending requests. @@ -266,18 +219,15 @@ def _remove_pending_request(self, future: Future[T], pending_requests: Dict[int, return seq return None - def _remove_pending_goal_request(self, - future: Future[ClientGoalHandle[GoalT, ResultT, FeedbackT]] - ) -> None: + def _remove_pending_goal_request(self, future): seq = self._remove_pending_request(future, self._pending_goal_requests) if seq in self._goal_sequence_number_to_goal_id: del self._goal_sequence_number_to_goal_id[seq] - def _remove_pending_cancel_request(self, future: Future[CancelGoal.Response]) -> None: + def _remove_pending_cancel_request(self, future): self._remove_pending_request(future, self._pending_cancel_requests) - def _remove_pending_result_request(self, future: Future[GetResultServiceResponse[ResultT]] - ) -> None: + def _remove_pending_result_request(self, future): seq = self._remove_pending_request(future, self._pending_result_requests) if seq in self._result_sequence_number_to_goal_id: goal_uuid = bytes(self._result_sequence_number_to_goal_id[seq].uuid) @@ -287,7 +237,7 @@ def _remove_pending_result_request(self, future: Future[GetResultServiceResponse del self._feedback_callbacks[goal_uuid] # Start Waitable API - def is_ready(self, wait_set: _rclpy.WaitSet) -> bool: + def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" ready_entities = self._client_handle.is_ready(wait_set) self._is_feedback_ready = ready_entities[0] @@ -297,9 +247,9 @@ def is_ready(self, wait_set: _rclpy.WaitSet) -> bool: self._is_result_response_ready = ready_entities[4] return any(ready_entities) - def take_data(self) -> 'ClientGoalHandleDict[ResultT, FeedbackT]': + def take_data(self) -> ClientGoalHandleDict: """Take stuff from lower level so the wait set doesn't immediately wake again.""" - data: 'ClientGoalHandleDict[ResultT, FeedbackT]' = {} + data: ClientGoalHandleDict = {} if self._is_goal_response_ready: taken_data = self._client_handle.take_goal_response( self._action_type.Impl.SendGoalService.Response) @@ -337,7 +287,7 @@ def take_data(self) -> 'ClientGoalHandleDict[ResultT, FeedbackT]': return data - async def execute(self, taken_data: 'ClientGoalHandleDict[ResultT, FeedbackT]') -> None: + async def execute(self, taken_data: ClientGoalHandleDict) -> None: """ Execute work after data has been taken from a ready wait set. @@ -400,9 +350,9 @@ async def execute(self, taken_data: 'ClientGoalHandleDict[ResultT, FeedbackT]') status = status_msg.status if goal_uuid in self._goal_handles: - status_goal_handle = self._goal_handles[goal_uuid]() - if status_goal_handle is not None: - status_goal_handle._status = status + goal_handle = self._goal_handles[goal_uuid]() + if goal_handle is not None: + goal_handle._status = status # Remove "done" goals from the list if (GoalStatus.STATUS_SUCCEEDED == status or GoalStatus.STATUS_CANCELED == status or @@ -412,26 +362,24 @@ async def execute(self, taken_data: 'ClientGoalHandleDict[ResultT, FeedbackT]') # Weak reference is None del self._goal_handles[goal_uuid] - def get_num_entities(self) -> NumberOfEntities: + def get_num_entities(self): """Return number of each type of entity used in the wait set.""" num_entities = self._client_handle.get_num_entities() return NumberOfEntities(*num_entities) - def add_to_wait_set(self, wait_set: _rclpy.WaitSet) -> None: + def add_to_wait_set(self, wait_set): """Add entities to wait set.""" self._client_handle.add_to_waitset(wait_set) - def __enter__(self) -> None: - self._client_handle.__enter__() + def __enter__(self): + return self._client_handle.__enter__() - def __exit__(self, t: Optional[Type[BaseException]], v: Optional[BaseException], - tb: Optional[TracebackType]) -> None: + def __exit__(self, t, v, tb): self._client_handle.__exit__(t, v, tb) # End Waitable API - def send_goal(self, goal: GoalT, **kwargs: 'Unpack[SendGoalKWargs]' - ) -> Optional[GetResultServiceResponse[ResultT]]: + def send_goal(self, goal, **kwargs): """ Send a goal and wait for the result. @@ -455,7 +403,7 @@ def send_goal(self, goal: GoalT, **kwargs: 'Unpack[SendGoalKWargs]' event = threading.Event() - def unblock(future: Future[Any]) -> None: + def unblock(future): nonlocal event event.set() @@ -463,24 +411,16 @@ def unblock(future: Future[Any]) -> None: send_goal_future.add_done_callback(unblock) event.wait() - exeception = send_goal_future.exception() - if exeception is not None: - raise exeception + if send_goal_future.exception() is not None: + raise send_goal_future.exception() goal_handle = send_goal_future.result() - if goal_handle is None: - return None result = self._get_result(goal_handle) return result - def send_goal_async( - self, - goal: GoalT, - feedback_callback: Optional[Callable[[FeedbackT], None]] = None, - goal_uuid: Optional[UUID] = None - ) -> Future[ClientGoalHandle[GoalT, ResultT, FeedbackT]]: + def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): """ Send a goal and asynchronously get the result. @@ -517,7 +457,7 @@ def send_goal_async( goal_uuid = bytes(request.goal_id.uuid) self._feedback_callbacks[goal_uuid] = feedback_callback - future: Future[ClientGoalHandle[GoalT, ResultT, FeedbackT]] = Future() + future = Future() self._pending_goal_requests[sequence_number] = future self._goal_sequence_number_to_goal_id[sequence_number] = request.goal_id future.add_done_callback(self._remove_pending_goal_request) @@ -526,8 +466,7 @@ def send_goal_async( return future - def _cancel_goal(self, goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] - ) -> Optional[CancelGoal.Response]: + def _cancel_goal(self, goal_handle): """ Send a cancel request for an active goal and wait for the response. @@ -539,7 +478,7 @@ def _cancel_goal(self, goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] """ event = threading.Event() - def unblock(future: Future[Any]) -> None: + def unblock(future): nonlocal event event.set() @@ -547,15 +486,11 @@ def unblock(future: Future[Any]) -> None: future.add_done_callback(unblock) event.wait() - exeception = future.exception() - if exeception is not None: - raise exeception + if future.exception() is not None: + raise future.exception() return future.result() - def _cancel_goal_async( - self, - goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] - ) -> Future[CancelGoal.Response]: + def _cancel_goal_async(self, goal_handle): """ Send a cancel request for an active goal and asynchronously get the result. @@ -575,7 +510,7 @@ def _cancel_goal_async( raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) - future: Future[CancelGoal.Response] = Future() + future = Future() self._pending_cancel_requests[sequence_number] = future future.add_done_callback(self._remove_pending_cancel_request) # Add future so executor is aware @@ -583,8 +518,7 @@ def _cancel_goal_async( return future - def _get_result(self, goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] - ) -> Optional[GetResultServiceResponse[ResultT]]: + def _get_result(self, goal_handle): """ Request the result for an active goal and wait for the response. @@ -596,7 +530,7 @@ def _get_result(self, goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] """ event = threading.Event() - def unblock(future: Future[Any]) -> None: + def unblock(future): nonlocal event event.set() @@ -604,13 +538,11 @@ def unblock(future: Future[Any]) -> None: future.add_done_callback(unblock) event.wait() - exception = future.exception() - if exception is not None: - raise exception + if future.exception() is not None: + raise future.exception() return future.result() - def _get_result_async(self, goal_handle: ClientGoalHandle[GoalT, ResultT, FeedbackT] - ) -> Future[GetResultServiceResponse[ResultT]]: + def _get_result_async(self, goal_handle): """ Request the result for an active goal asynchronously. @@ -630,7 +562,7 @@ def _get_result_async(self, goal_handle: ClientGoalHandle[GoalT, ResultT, Feedba raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) - future: Future[GetResultServiceResponse[ResultT]] = Future() + future = Future() self._pending_result_requests[sequence_number] = future self._result_sequence_number_to_goal_id[sequence_number] = result_request.goal_id future.add_done_callback(self._remove_pending_result_request) @@ -639,7 +571,7 @@ def _get_result_async(self, goal_handle: ClientGoalHandle[GoalT, ResultT, Feedba return future - def server_is_ready(self) -> bool: + def server_is_ready(self): """ Check if there is an action server ready to process requests from this client. @@ -648,7 +580,7 @@ def server_is_ready(self) -> bool: with self._node.handle: return self._client_handle.is_action_server_available() - def wait_for_server(self, timeout_sec: Optional[float] = None) -> bool: + def wait_for_server(self, timeout_sec=None): """ Wait for an action sever to be ready. @@ -669,7 +601,7 @@ def wait_for_server(self, timeout_sec: Optional[float] = None) -> bool: return self.server_is_ready() - def destroy(self) -> None: + def destroy(self): """Destroy the underlying action client handle.""" self._client_handle.destroy_when_not_in_use() self._node.remove_waitable(self) diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index ce60309a6..8296df76c 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -17,44 +17,18 @@ import threading import traceback -from types import TracebackType -from typing import (Any, Callable, Dict, Generic, Literal, Optional, Tuple, Type, - TYPE_CHECKING, TypedDict, TypeVar) - +from typing import Any, TypedDict from action_msgs.msg import GoalInfo, GoalStatus -from action_msgs.srv._cancel_goal import CancelGoal + from rclpy.executors import await_or_execute from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import qos_profile_action_status_default from rclpy.qos import qos_profile_services_default from rclpy.qos import QoSProfile from rclpy.task import Future -from rclpy.type_support import (Action, check_for_type_support, FeedbackMessage, FeedbackT, - GetResultServiceRequest, GetResultServiceResponse, GoalT, ResultT, - SendGoalServiceRequest) +from rclpy.type_support import check_for_type_support from rclpy.waitable import NumberOfEntities, Waitable -from unique_identifier_msgs.msg import UUID - - -if TYPE_CHECKING: - from typing_extensions import TypeAlias - - from rclpy.callback_groups import CallbackGroup - from rclpy.node import Node - - ServerGoalHandleDictGoalT = TypeVar('ServerGoalHandleDictGoalT') - - class ServerGoalHandleDict(TypedDict, - Generic[ServerGoalHandleDictGoalT], - total=False): - goal: Tuple['_rclpy.rmw_request_id_t', SendGoalServiceRequest[ServerGoalHandleDictGoalT]] - cancel: Tuple['_rclpy.rmw_request_id_t', CancelGoal.Request] - result: Tuple['_rclpy.rmw_request_id_t', GetResultServiceRequest] - expired: Tuple[GoalInfo, ...] -else: - ServerGoalHandleDict: 'TypeAlias' = Dict[str, object] - # Re-export exception defined in _rclpy C extension. RCLError = _rclpy.RCLError @@ -77,15 +51,17 @@ class CancelResponse(Enum): GoalEvent = _rclpy.GoalEvent -class ServerGoalHandle(Generic[GoalT, ResultT, FeedbackT]): +class ServerGoalHandleDict(TypedDict, total=False): + goal: Any + cancel: Any + result: Any + expired: Any + + +class ServerGoalHandle: """Goal handle for working with Action Servers.""" - def __init__( - self, - action_server: 'ActionServer[GoalT, ResultT, FeedbackT]', - goal_info: GoalInfo, - goal_request: GoalT - ) -> None: + def __init__(self, action_server, goal_info, goal_request): """ Accept a new goal with the given action server. @@ -105,43 +81,39 @@ def __init__( self._cancel_requested = False self._lock = threading.Lock() - def __eq__(self, other: object) -> bool: - if isinstance(other, ServerGoalHandle): - return self.goal_id == other.goal_id - return False + def __eq__(self, other): + return self.goal_id == other.goal_id - def __ne__(self, other: object) -> bool: - if isinstance(other, ServerGoalHandle): - return self.goal_id != other.goal_id - return True + def __ne__(self, other): + return self.goal_id != other.goal_id @property - def request(self) -> GoalT: + def request(self): return self._goal_request @property - def goal_id(self) -> UUID: + def goal_id(self): return self._goal_info.goal_id @property - def is_active(self) -> bool: + def is_active(self): with self._lock: if self._goal_handle is None: return False return self._goal_handle.is_active() @property - def is_cancel_requested(self) -> bool: + def is_cancel_requested(self): return GoalStatus.STATUS_CANCELING == self.status @property - def status(self) -> int: + def status(self): with self._lock: if self._goal_handle is None: return GoalStatus.STATUS_UNKNOWN return self._goal_handle.get_status() - def _update_state(self, event: _rclpy.GoalEvent) -> None: + def _update_state(self, event): with self._lock: # Ignore updates for already destructed goal handles if self._goal_handle is None: @@ -157,11 +129,7 @@ def _update_state(self, event: _rclpy.GoalEvent) -> None: if not self._goal_handle.is_active(): self._action_server.notify_goal_done() - def execute( - self, - execute_callback: Optional[Callable[['ServerGoalHandle[GoalT, ResultT, FeedbackT]'], - ResultT]] = None - ) -> None: + def execute(self, execute_callback=None): # It's possible that there has been a request to cancel the goal prior to executing. # In this case we want to avoid the illegal state transition to EXECUTING # but still call the users execute callback to let them handle canceling the goal. @@ -169,7 +137,7 @@ def execute( self._update_state(_rclpy.GoalEvent.EXECUTE) self._action_server.notify_execute(self, execute_callback) - def publish_feedback(self, feedback: FeedbackMessage[FeedbackT]) -> None: + def publish_feedback(self, feedback): if not isinstance(feedback, self._action_server.action_type.Feedback): raise TypeError() @@ -187,16 +155,16 @@ def publish_feedback(self, feedback: FeedbackMessage[FeedbackT]) -> None: # Publish self._action_server._handle.publish_feedback(feedback_message) - def succeed(self) -> None: + def succeed(self): self._update_state(_rclpy.GoalEvent.SUCCEED) - def abort(self) -> None: + def abort(self): self._update_state(_rclpy.GoalEvent.ABORT) - def canceled(self) -> None: + def canceled(self): self._update_state(_rclpy.GoalEvent.CANCELED) - def destroy(self) -> None: + def destroy(self): with self._lock: if self._goal_handle is None: return @@ -204,47 +172,42 @@ def destroy(self) -> None: self._goal_handle = None -def default_handle_accepted_callback(goal_handle: ServerGoalHandle[Any, Any, Any]) -> None: +def default_handle_accepted_callback(goal_handle): """Execute the goal.""" goal_handle.execute() -def default_goal_callback( - goal_request: SendGoalServiceRequest[Any] -) -> Literal[GoalResponse.ACCEPT]: +def default_goal_callback(goal_request): """Accept all goals.""" return GoalResponse.ACCEPT -def default_cancel_callback(cancel_request: CancelGoal.Request) -> Literal[CancelResponse.REJECT]: +def default_cancel_callback(cancel_request): """No cancellations.""" return CancelResponse.REJECT -class ActionServer(Generic[GoalT, ResultT, FeedbackT], Waitable['ServerGoalHandleDict[GoalT]']): +class ActionServer(Waitable[ServerGoalHandleDict]): """ROS Action server.""" def __init__( self, - node: 'Node', - action_type: Type[Action[GoalT, ResultT, FeedbackT]], - action_name: str, - execute_callback: Callable[[ServerGoalHandle[GoalT, ResultT, FeedbackT]], ResultT], + node, + action_type, + action_name, + execute_callback, *, - callback_group: 'Optional[CallbackGroup]' = None, - goal_callback: Callable[[CancelGoal.Request], GoalResponse] = default_goal_callback, - handle_accepted_callback: Callable[[ServerGoalHandle[GoalT, - ResultT, - FeedbackT]], - None] = default_handle_accepted_callback, - cancel_callback: Callable[[CancelGoal.Request], CancelResponse] = default_cancel_callback, - goal_service_qos_profile: QoSProfile = qos_profile_services_default, - result_service_qos_profile: QoSProfile = qos_profile_services_default, - cancel_service_qos_profile: QoSProfile = qos_profile_services_default, - feedback_pub_qos_profile: QoSProfile = QoSProfile(depth=10), - status_pub_qos_profile: QoSProfile = qos_profile_action_status_default, - result_timeout: int = 10 - ) -> None: + callback_group=None, + goal_callback=default_goal_callback, + handle_accepted_callback=default_handle_accepted_callback, + cancel_callback=default_cancel_callback, + goal_service_qos_profile=qos_profile_services_default, + result_service_qos_profile=qos_profile_services_default, + cancel_service_qos_profile=qos_profile_services_default, + feedback_pub_qos_profile=QoSProfile(depth=10), + status_pub_qos_profile=qos_profile_action_status_default, + result_timeout=10 + ): """ Create an ActionServer. @@ -300,20 +263,16 @@ def __init__( ) # key: UUID in bytes, value: GoalHandle - self._goal_handles: Dict[bytes, ServerGoalHandle[GoalT, ResultT, FeedbackT]] = {} + self._goal_handles = {} # key: UUID in bytes, value: Future - self._result_futures: Dict[bytes, Future[GetResultServiceResponse[ResultT]]] = {} + self._result_futures = {} callback_group.add_entity(self) self._node.add_waitable(self) self._logger = self._node.get_logger().get_child('action_server') - async def _execute_goal_request( - self, - request_header_and_message: Tuple['_rclpy.rmw_request_id_t', - SendGoalServiceRequest[GoalT]] - ) -> None: + async def _execute_goal_request(self, request_header_and_message): request_header, goal_request = request_header_and_message goal_uuid = goal_request.goal_id goal_info = GoalInfo() @@ -374,11 +333,7 @@ async def _execute_goal_request( # Provide the user a reference to the goal handle await await_or_execute(self._handle_accepted_callback, goal_handle) - async def _execute_goal( - self, - execute_callback: Callable[[ServerGoalHandle[GoalT, ResultT, FeedbackT]], ResultT], - goal_handle: ServerGoalHandle[GoalT, ResultT, FeedbackT] - ) -> None: + async def _execute_goal(self, execute_callback, goal_handle): goal_uuid = goal_handle.goal_id.uuid self._logger.debug('Executing goal with ID {0}'.format(goal_uuid)) @@ -406,10 +361,7 @@ async def _execute_goal( result_response.result = execute_result self._result_futures[bytes(goal_uuid)].set_result(result_response) - async def _execute_cancel_request( - self, - request_header_and_message: Tuple['_rclpy.rmw_request_id_t', CancelGoal.Request] - ) -> None: + async def _execute_cancel_request(self, request_header_and_message): request_header, cancel_request = request_header_and_message self._logger.debug('Cancel request received: {0}'.format(cancel_request)) @@ -451,10 +403,7 @@ async def _execute_cancel_request( except RCLError: self._logger.warn('Failed to send cancel response (the client may have gone away)') - async def _execute_get_result_request( - self, - request_header_and_message: Tuple['_rclpy.rmw_request_id_t', GetResultServiceRequest] - ) -> None: + async def _execute_get_result_request(self, request_header_and_message): request_header, result_request = request_header_and_message goal_uuid = result_request.goal_id.uuid @@ -475,7 +424,7 @@ async def _execute_get_result_request( self._result_futures[bytes(goal_uuid)].add_done_callback( functools.partial(self._send_result_response, request_header)) - async def _execute_expire_goals(self, expired_goals: Tuple[GoalInfo, ...]) -> None: + async def _execute_expire_goals(self, expired_goals): for goal in expired_goals: goal_uuid = bytes(goal.goal_id.uuid) self._goal_handles[goal_uuid].destroy() @@ -483,11 +432,7 @@ async def _execute_expire_goals(self, expired_goals: Tuple[GoalInfo, ...]) -> No self.remove_future(self._result_futures[goal_uuid]) del self._result_futures[goal_uuid] - def _send_result_response( - self, - request_header: '_rclpy.rmw_request_id_t', - future: Future[GetResultServiceResponse[ResultT]] - ) -> None: + def _send_result_response(self, request_header, future): try: # If the client goes away anytime before this, sending the result response may fail. # Catch the exception here and go on so we don't crash. @@ -496,11 +441,11 @@ def _send_result_response( self._logger.warn('Failed to send result response (the client may have gone away)') @property - def action_type(self) -> Type[Action[GoalT, ResultT, FeedbackT]]: + def action_type(self): return self._action_type # Start Waitable API - def is_ready(self, wait_set: _rclpy.WaitSet) -> bool: + def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" with self._lock: ready_entities = self._handle.is_ready(wait_set) @@ -510,9 +455,9 @@ def is_ready(self, wait_set: _rclpy.WaitSet) -> bool: self._is_goal_expired = ready_entities[3] return any(ready_entities) - def take_data(self) -> 'ServerGoalHandleDict[GoalT]': + def take_data(self) -> ServerGoalHandleDict: """Take stuff from lower level so the wait set doesn't immediately wake again.""" - data: 'ServerGoalHandleDict[GoalT]' = {} + data: ServerGoalHandleDict = {} if self._is_goal_request_ready: with self._lock: taken_data = self._handle.take_goal_request( @@ -546,7 +491,7 @@ def take_data(self) -> 'ServerGoalHandleDict[GoalT]': return data - async def execute(self, taken_data: 'ServerGoalHandleDict[GoalT]') -> None: + async def execute(self, taken_data: ServerGoalHandleDict) -> None: """ Execute work after data has been taken from a ready wait set. @@ -565,7 +510,7 @@ async def execute(self, taken_data: 'ServerGoalHandleDict[GoalT]') -> None: if 'expired' in taken_data: await self._execute_expire_goals(taken_data['expired']) - def get_num_entities(self) -> NumberOfEntities: + def get_num_entities(self): """Return number of each type of entity used in the wait set.""" num_entities = self._handle.get_num_entities() return NumberOfEntities( @@ -575,26 +520,20 @@ def get_num_entities(self) -> NumberOfEntities: num_entities[3], num_entities[4]) - def add_to_wait_set(self, wait_set: _rclpy.WaitSet) -> None: + def add_to_wait_set(self, wait_set): """Add entities to wait set.""" with self._lock: self._handle.add_to_waitset(wait_set) - def __enter__(self) -> None: - self._handle.__enter__() + def __enter__(self): + return self._handle.__enter__() - def __exit__(self, t: Optional[Type[BaseException]], - v: Optional[BaseException], tb: Optional[TracebackType]) -> None: + def __exit__(self, t, v, tb): self._handle.__exit__(t, v, tb) # End Waitable API - def notify_execute( - self, - goal_handle: ServerGoalHandle[GoalT, ResultT, FeedbackT], - execute_callback: Optional[Callable[[ServerGoalHandle[GoalT, ResultT, FeedbackT]], - ResultT]] - ) -> None: + def notify_execute(self, goal_handle, execute_callback): # Use provided callback, defaulting to a previously registered callback if execute_callback is None: if self._execute_callback is None: @@ -602,18 +541,13 @@ def notify_execute( execute_callback = self._execute_callback # Schedule user callback for execution - if self._node.executor: - self._node.executor.create_task(self._execute_goal, execute_callback, goal_handle) + self._node.executor.create_task(self._execute_goal, execute_callback, goal_handle) - def notify_goal_done(self) -> None: + def notify_goal_done(self): with self._lock: self._handle.notify_goal_done() - def register_handle_accepted_callback( - self, - handle_accepted_callback: Optional[Callable[[ - ServerGoalHandle[GoalT, ResultT, FeedbackT]], None]] - ) -> None: + def register_handle_accepted_callback(self, handle_accepted_callback): """ Register a callback for handling newly accepted goals. @@ -634,10 +568,7 @@ def register_handle_accepted_callback( handle_accepted_callback = default_handle_accepted_callback self._handle_accepted_callback = handle_accepted_callback - def register_goal_callback( - self, - goal_callback: Optional[Callable[[SendGoalServiceRequest[GoalT]], GoalResponse]] - ) -> None: + def register_goal_callback(self, goal_callback): """ Register a callback for handling new goal requests. @@ -656,10 +587,7 @@ def register_goal_callback( goal_callback = default_goal_callback self._goal_callback = goal_callback - def register_cancel_callback( - self, - cancel_callback: Optional[Callable[[CancelGoal.Request], CancelResponse]] - ) -> None: + def register_cancel_callback(self, cancel_callback): """ Register a callback for handling cancel requests. @@ -678,10 +606,7 @@ def register_cancel_callback( cancel_callback = default_cancel_callback self._cancel_callback = cancel_callback - def register_execute_callback( - self, - execute_callback: Callable[[ServerGoalHandle[GoalT, ResultT, FeedbackT]], ResultT] - ) -> None: + def register_execute_callback(self, execute_callback): """ Register a callback for executing action goals. @@ -700,7 +625,7 @@ def register_execute_callback( raise TypeError('Failed to register goal execution callback: not callable') self._execute_callback = execute_callback - def destroy(self) -> None: + def destroy(self): """Destroy the underlying action server handle.""" for goal_handle in self._goal_handles.values(): goal_handle.destroy() diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index 60c38f719..cba396a1f 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -28,7 +28,7 @@ from rclpy.qos import QoSProfile from rclpy.service_introspection import ServiceIntrospectionState from rclpy.task import Future -from rclpy.type_support import Srv, SrvRequestT, SrvResponseT +from rclpy.type_support import Srv, SrvEventT, SrvRequestT, SrvResponseT # Left To Support Legacy TypeVars SrvType = TypeVar('SrvType') @@ -36,12 +36,12 @@ SrvTypeResponse = TypeVar('SrvTypeResponse') -class Client(Generic[SrvRequestT, SrvResponseT]): +class Client(Generic[SrvRequestT, SrvResponseT, SrvEventT]): def __init__( self, context: Context, client_impl: _rclpy.Client, - srv_type: Type[Srv[SrvRequestT, SrvResponseT]], + srv_type: Type[Srv[SrvRequestT, SrvResponseT, SrvEventT]], srv_name: str, qos_profile: QoSProfile, callback_group: CallbackGroup @@ -231,7 +231,7 @@ def destroy(self) -> None: """ self.__client.destroy_when_not_in_use() - def __enter__(self) -> 'Client[SrvRequestT, SrvResponseT]': + def __enter__(self) -> 'Client[SrvRequestT, SrvResponseT, SrvEventT]': return self def __exit__( diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index 612831668..679f6fa4f 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -18,17 +18,12 @@ from enum import Enum, IntEnum from types import TracebackType from typing import Any, Generic, Literal, overload, Sequence, TypedDict - -from action_msgs.msg import GoalInfo -from action_msgs.msg._goal_status_array import GoalStatusArray -from action_msgs.srv._cancel_goal import CancelGoal from rclpy.clock import JumpHandle from rclpy.clock_type import ClockType from rclpy.duration import Duration from rclpy.parameter import Parameter from rclpy.subscription import MessageInfo -from type_support import (MsgT, Action, GoalT, ResultT, FeedbackT, SendGoalServiceResponse, - GetResultServiceResponse, FeedbackMessage, SendGoalServiceRequest, GetResultServiceRequest) +from rclpy.type_support import MsgT def rclpy_remove_ros_args(pycli_args: Sequence[str]) -> list[str]: @@ -448,191 +443,6 @@ class WaitSet(Destroyable): """Wait until timeout is reached or event happened.""" -class ActionClient(Generic[GoalT, ResultT, FeedbackT], Destroyable): - - def __init__( - self, - node: Node, - pyaction_type: type[Action[GoalT, ResultT, FeedbackT]], - goal_service_qos: rmw_qos_profile_t, - result_service_qos: rmw_qos_profile_t, - cancel_service_qos: rmw_qos_profile_t, - feedback_service_qos: rmw_qos_profile_t, - status_topci_qos: rmw_qos_profile_t - ) -> None: ... - - @property - def pointer(self) -> int: - """Get the address of the entity as an integer.""" - - def take_goal_response(self, pymsg_type: type[SendGoalServiceResponse] - ) -> tuple[int, SendGoalServiceResponse] | tuple[None, None]: - """Take an action goal response.""" - - def send_result_request(self, pyrequest: GetResultServiceRequest) -> int: - """Send an action result requst.""" - - def take_cancel_response(self, pymsg_type: type[CancelGoal.Response] - ) -> tuple[int, CancelGoal.Response] | tuple[None, None]: - """Take an action cancel response.""" - - def take_feedback(self, pymsg_type: type[FeedbackMessage[FeedbackT]] - ) -> FeedbackMessage[FeedbackT] | None: - """Take a feedback message from a given action client.""" - - def send_cancel_request(self: CancelGoal.Request) -> int: - """Send an action cancel request.""" - - def send_goal_request(self: SendGoalServiceRequest[GoalT]) -> int: - """Send an action goal request.""" - - def take_result_response(self, pymsg_type: type[GetResultServiceResponse[ResultT]] - ) -> tuple[int, GetResultServiceResponse[ResultT]] | tuple[None, None]: - """Take an action result response.""" - - def get_num_entities(self) -> tuple[int, int, int, int, int]: - """Get the number of wait set entities that make up an action entity.""" - - def is_action_server_available(self) -> bool: - """Check if an action server is available for the given action client.""" - - def add_to_waitset(self, waitset: WaitSet) -> None: - """Add an action entity to a wait set.""" - - def is_ready(self) -> bool: - """Check if an action entity has any ready wait set entities.""" - - def take_status(self, pymsg_type: type[GoalStatusArray]) -> GoalStatusArray | None: - """Take an action status response.""" - - -class GoalEvent(Enum): - _value_: int - EXECUTE = ... - CANCEL_GOAL = ... - SUCCEED = ... - ABORT = ... - CANCELED = ... - - -class rmw_request_id_t: - writer_guid: list[int] - sequence_number: int - - -class ActionServer(Generic[GoalT, ResultT, FeedbackT], Destroyable): - - def __init__( - self, - node: Node, - rclpy_clock: Clock, - pyaction_type: type[Action[GoalT, ResultT, FeedbackT]], - action_name: str, - goal_service_qos: rmw_qos_profile_t, - result_service_qos: rmw_qos_profile_t, - cancel_service_qos: rmw_qos_profile_t, - feedback_topic_qos: rmw_qos_profile_t, - status_topic_qos: rmw_qos_profile_t, - result_timeout: float - ) -> None: ... - - @property - def pointer(self) -> int: - """Get the address of the entity as an integer.""" - - def take_goal_request( - self, - pymsg_type: type[SendGoalServiceRequest[GoalT]] - ) -> tuple[rmw_request_id_t, SendGoalServiceRequest[GoalT]] | tuple[None, None]: - """Take an action goal request.""" - - def send_goal_response( - self, - header: rmw_request_id_t, - pyresponse: SendGoalServiceResponse - ) -> None: - """Send an action goal response.""" - - def send_result_response( - self, - header: rmw_request_id_t, - pyresponse: GetResultServiceResponse[ResultT] - ) -> None: - """Send an action result response.""" - - def take_cancel_request( - self, - pymsg_type: type[CancelGoal.Request] - ) -> tuple[rmw_request_id_t, CancelGoal.Request] | tuple[None, None]: - """Take an action cancel request.""" - - def take_result_request( - self, - pymsg_type: type[GetResultServiceRequest] - ) -> tuple[rmw_request_id_t, GetResultServiceRequest] | tuple[None, None]: - """Take an action result request.""" - - def send_cancel_response( - self, - header: rmw_request_id_t, - pyresponse: int - ) -> None: - """Send an action cancel response.""" - - def publish_feedback( - self, - pymsg: FeedbackMessage[FeedbackT] - ) -> None: - """Publish a feedback message from a given action server.""" - - def publish_status(self) -> None: - """Publish a status message from a given action server.""" - - def notify_goal_done(self) -> None: - """Notify goal is done.""" - - def goal_exists(self, pygoal_info: GoalInfo) -> bool: - """Check is a goal exists in the server.""" - - def process_cancel_request( - self, - pycancel_request: CancelGoal.Request, - pycancel_response_tpye: type[CancelGoal.Response] - ) -> CancelGoal.Response: - """Process a cancel request""" - - def expire_goals(self, max_num_goals: int) -> tuple[GoalInfo, ...]: - """Expired goals.""" - - def get_num_entities(self) -> tuple[int, int, int, int, int]: - """Get the number of wait set entities that make up an action entity.""" - - def is_ready(self, wait_set: WaitSet) -> tuple[bool, bool, bool, bool]: - """Check if an action entity has any ready wait set entities.""" - - def add_to_waitset(self, wait_set: WaitSet) -> None: - """Add an action entity to a wait set.""" - - -class ActionGoalHandle: - - def __init__(self, action_server: ActionServer, pygoal_info_msg: GoalInfo) -> None: - ... - - @property - def pointer(self) -> int: - """Get the address of the entity as an integer.""" - - def get_status(self) -> GoalEvent: - """Get the status of a goal.""" - - def update_goal_state(self, event: GoalEvent) -> None: - """Update a goal state.""" - - def is_active(self) -> bool: - """Check if a goal is active.""" - - class RCLError(RuntimeError): pass @@ -644,7 +454,7 @@ class NodeNameNonExistentError(RCLError): class InvalidHandle(RuntimeError): pass - + class SignalHandlerOptions(Enum): _value_: int NO = ... diff --git a/rclpy/rclpy/impl/implementation_singleton.pyi b/rclpy/rclpy/impl/implementation_singleton.pyi index 521e06e98..a1e16bdf9 100644 --- a/rclpy/rclpy/impl/implementation_singleton.pyi +++ b/rclpy/rclpy/impl/implementation_singleton.pyi @@ -13,6 +13,6 @@ # limitations under the License. -from impl import _rclpy_pybind11 +from rclpy.impl import _rclpy_pybind11 rclpy_implementation = _rclpy_pybind11 diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 2e7bbd26a..8da2faafc 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -89,6 +89,7 @@ from rclpy.type_support import check_is_valid_srv_type from rclpy.type_support import MsgT from rclpy.type_support import Srv +from rclpy.type_support import SrvEventT from rclpy.type_support import SrvRequestT from rclpy.type_support import SrvResponseT from rclpy.utilities import get_default_context @@ -172,8 +173,8 @@ def __init__( self._parameters: Dict[str, Parameter[Any]] = {} self._publishers: List[Publisher[Any]] = [] self._subscriptions: List[Subscription[Any]] = [] - self._clients: List[Client[Any, Any]] = [] - self._services: List[Service[Any, Any]] = [] + self._clients: List[Client[Any, Any, Any]] = [] + self._services: List[Service[Any, Any, Any]] = [] self._timers: List[Timer] = [] self._guards: List[GuardCondition] = [] self.__waitables: List[Waitable[Any]] = [] @@ -269,12 +270,12 @@ def subscriptions(self) -> Iterator[Subscription[Any]]: yield from self._subscriptions @property - def clients(self) -> Iterator[Client[Any, Any]]: + def clients(self) -> Iterator[Client[Any, Any, Any]]: """Get clients that have been created on this node.""" yield from self._clients @property - def services(self) -> Iterator[Service[Any, Any]]: + def services(self) -> Iterator[Service[Any, Any, Any]]: """Get services that have been created on this node.""" yield from self._services @@ -1689,12 +1690,12 @@ def create_subscription( def create_client( self, - srv_type: Type[Srv[SrvRequestT, SrvResponseT]], + srv_type: Type[Srv[SrvRequestT, SrvResponseT, SrvEventT]], srv_name: str, *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: Optional[CallbackGroup] = None - ) -> Client[SrvRequestT, SrvResponseT]: + ) -> Client[SrvRequestT, SrvResponseT, SrvEventT]: """ Create a new service client. @@ -1731,13 +1732,13 @@ def create_client( def create_service( self, - srv_type: Type[Srv[SrvRequestT, SrvResponseT]], + srv_type: Type[Srv[SrvRequestT, SrvResponseT, SrvEventT]], srv_name: str, callback: Callable[[SrvRequestT, SrvResponseT], SrvResponseT], *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: Optional[CallbackGroup] = None - ) -> Service[SrvRequestT, SrvResponseT]: + ) -> Service[SrvRequestT, SrvResponseT, SrvEventT]: """ Create a new service server. @@ -1891,7 +1892,7 @@ def destroy_subscription(self, subscription: Subscription[Any]) -> bool: return True return False - def destroy_client(self, client: Client[Any, Any]) -> bool: + def destroy_client(self, client: Client[Any, Any, Any]) -> bool: """ Destroy a service client created by the node. @@ -1907,7 +1908,7 @@ def destroy_client(self, client: Client[Any, Any]) -> bool: return True return False - def destroy_service(self, service: Service[Any, Any]) -> bool: + def destroy_service(self, service: Service[Any, Any, Any]) -> bool: """ Destroy a service server created by the node. diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index de1c7d198..bc13940cf 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -24,7 +24,7 @@ from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.service_introspection import ServiceIntrospectionState -from rclpy.type_support import Srv, SrvRequestT, SrvResponseT +from rclpy.type_support import Srv, SrvEventT, SrvRequestT, SrvResponseT # Used for documentation purposes only SrvType = TypeVar('SrvType') @@ -32,11 +32,11 @@ SrvTypeResponse = TypeVar('SrvTypeResponse') -class Service(Generic[SrvRequestT, SrvResponseT]): +class Service(Generic[SrvRequestT, SrvResponseT, SrvEventT]): def __init__( self, service_impl: _rclpy.Service, - srv_type: Type[Srv[SrvRequestT, SrvResponseT]], + srv_type: Type[Srv[SrvRequestT, SrvResponseT, SrvEventT]], srv_name: str, callback: Callable[[SrvRequestT, SrvResponseT], SrvResponseT], callback_group: CallbackGroup, @@ -121,7 +121,7 @@ def destroy(self) -> None: """ self.__service.destroy_when_not_in_use() - def __enter__(self) -> 'Service[SrvRequestT, SrvResponseT]': + def __enter__(self) -> 'Service[SrvRequestT, SrvResponseT, SrvEventT]': return self def __exit__( diff --git a/rclpy/rclpy/type_support.py b/rclpy/rclpy/type_support.py index cbdde9e6b..33ef95d9d 100644 --- a/rclpy/rclpy/type_support.py +++ b/rclpy/rclpy/type_support.py @@ -12,19 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Any, ClassVar, Iterable, Optional, Protocol, Type, TYPE_CHECKING, TypeVar, Union +from typing import NoReturn, Optional, Protocol, Type, TypeVar, Union - -from action_msgs.msg._goal_status_array import GoalStatusArray -from action_msgs.srv._cancel_goal import CancelGoal -from builtin_interfaces.msg import Time from rclpy.exceptions import NoTypeSupportImportedException -from service_msgs.msg._service_event_info import ServiceEventInfo -from unique_identifier_msgs.msg import UUID - - -if TYPE_CHECKING: - from typing_extensions import TypeAlias class PyCapsule(Protocol): @@ -40,7 +30,7 @@ class PyCapsule(Protocol): class CommonMsgSrvMetaClass(ProtocolType): """Shared attributes between messages and services.""" - _TYPE_SUPPORT: ClassVar[Optional[PyCapsule]] + _TYPE_SUPPORT: Optional[PyCapsule] @classmethod def __import_type_support__(cls) -> None: @@ -50,10 +40,10 @@ def __import_type_support__(cls) -> None: class MsgMetaClass(CommonMsgSrvMetaClass): """Generic Message Metaclass Alias.""" - _CREATE_ROS_MESSAGE: ClassVar[Optional[PyCapsule]] - _CONVERT_FROM_PY: ClassVar[Optional[PyCapsule]] - _CONVERT_TO_PY: ClassVar[Optional[PyCapsule]] - _DESTROY_ROS_MESSAGE: ClassVar[Optional[PyCapsule]] + _CREATE_ROS_MESSAGE: Optional[PyCapsule] + _CONVERT_FROM_PY: Optional[PyCapsule] + _CONVERT_TO_PY: Optional[PyCapsule] + _DESTROY_ROS_MESSAGE: Optional[PyCapsule] class Msg(Protocol, metaclass=MsgMetaClass): @@ -66,84 +56,24 @@ class Msg(Protocol, metaclass=MsgMetaClass): SrvRequestT = TypeVar('SrvRequestT', bound=Msg) SrvResponseT = TypeVar('SrvResponseT', bound=Msg) +SrvEventT = TypeVar('SrvEventT', bound=Msg) -class EventMessage(Msg, Protocol[SrvRequestT, SrvResponseT]): - info: ServiceEventInfo - request: Iterable[SrvRequestT] - response: Iterable[SrvResponseT] - - -class Srv(Protocol[SrvRequestT, SrvResponseT], metaclass=CommonMsgSrvMetaClass): +class Srv(Protocol[SrvRequestT, SrvResponseT, SrvEventT], metaclass=CommonMsgSrvMetaClass): """Generic Service Type Alias.""" Request: Type[SrvRequestT] Response: Type[SrvResponseT] - Event: Type[EventMessage[SrvRequestT, SrvResponseT]] - - -GoalT = TypeVar('GoalT', bound=Msg) -ResultT = TypeVar('ResultT', bound=Msg) -FeedbackT = TypeVar('FeedbackT', bound=Msg) - - -class SendGoalServiceRequest(Msg, Protocol[GoalT]): - goal_id: UUID - goal: GoalT - - -class SendGoalServiceResponse(Msg, Protocol): - accepted: bool - stamp: Time - - -SendGoalService: 'TypeAlias' = Srv[SendGoalServiceRequest[GoalT], SendGoalServiceResponse] - - -class GetResultServiceRequest(Msg, Protocol): - goal_id: UUID - - -class GetResultServiceResponse(Msg, Protocol[ResultT]): - status: int - result: ResultT - - -GetResultService: 'TypeAlias' = Srv[GetResultServiceRequest, GetResultServiceResponse[ResultT]] - - -class FeedbackMessage(Msg, Protocol[FeedbackT]): - goal_id: UUID - feedback: FeedbackT - - -class Impl(Protocol[GoalT, ResultT, FeedbackT]): - - SendGoalService: Type[SendGoalService[GoalT]] - GetResultService: Type[GetResultService[ResultT]] - FeedbackMessage: Type[FeedbackMessage[FeedbackT]] - CancelGoalService: ClassVar[Type[CancelGoal]] - GoalStatusMessage: ClassVar[Type[GoalStatusArray]] - - -class Action(Protocol[GoalT, - ResultT, - FeedbackT], - metaclass=CommonMsgSrvMetaClass): - Goal: Type[GoalT] - Result: Type[ResultT] - Feedback: Type[FeedbackT] + Event: Type[SrvEventT] - Impl: Type[Impl[GoalT, ResultT, FeedbackT]] + def __init__(self) -> NoReturn: ... # Can be used if https://github.com/python/typing/issues/548 ever gets approved. -SrvT = TypeVar('SrvT', bound=Srv) -ActionT = TypeVar('ActionT', bound=Action) +SrvT = TypeVar('SrvT', bound=Type[Srv]) -def check_for_type_support(msg_or_srv_type: Type[Union[Msg, Srv[Any, Any], - Action[Any, Any, Any]]]) -> None: +def check_for_type_support(msg_or_srv_type: Type[Union[Msg, Srv]]) -> None: try: ts = msg_or_srv_type._TYPE_SUPPORT except AttributeError as e: @@ -175,7 +105,7 @@ def check_is_valid_msg_type(msg_type: Type[Msg]) -> None: ) from None -def check_is_valid_srv_type(srv_type: Type[Srv[Any, Any]]) -> None: +def check_is_valid_srv_type(srv_type: Type[Srv]) -> None: check_for_type_support(srv_type) try: assert None not in ( diff --git a/rclpy/rclpy/waitable.py b/rclpy/rclpy/waitable.py index ae1e905a0..56b363df5 100644 --- a/rclpy/rclpy/waitable.py +++ b/rclpy/rclpy/waitable.py @@ -22,6 +22,8 @@ if TYPE_CHECKING: + from typing_extensions import Self + from rclpy.callback_groups import CallbackGroup from rclpy.task import Future @@ -89,7 +91,7 @@ def __init__(self, callback_group: 'CallbackGroup'): # List of Futures that have callbacks needing execution self._futures: List[Future[Any]] = [] - def __enter__(self) -> None: + def __enter__(self) -> 'Self': """Implement to mark entities as in-use to prevent destruction while waiting on them.""" raise NotImplementedError('Must be implemented by subclass') diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp index 9256bccf2..a899211fc 100644 --- a/rclpy/src/rclpy/action_server.cpp +++ b/rclpy/src/rclpy/action_server.cpp @@ -400,7 +400,7 @@ define_action_server(py::object module) "Send an action cancel response.") .def( "publish_feedback", &ActionServer::publish_feedback, - "Publish a feedback message from a given action server.") + " Publish a feedback message from a given action server.") .def( "publish_status", &ActionServer::publish_status, "Publish a status message from a given action server.")