Skip to content
155 changes: 87 additions & 68 deletions rclpy/rclpy/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,92 @@
SrvTypeResponse = TypeVar('SrvTypeResponse')


class Client(Generic[SrvRequestT, SrvResponseT]):
class BaseClient(Generic[SrvRequestT, SrvResponseT]):
def __init__(
self,
context: Context,
client_impl: '_rclpy.Client[SrvRequestT, SrvResponseT]',
srv_type: type[Srv[SrvRequestT, SrvResponseT]],
srv_name: str,
qos_profile: QoSProfile,
) -> None:
self.context = context
self.__client = client_impl
self.srv_type = srv_type
self.srv_name = srv_name
self.qos_profile = qos_profile

def service_is_ready(self) -> bool:
"""
Check if there is a service server ready.

:return: ``True`` if a server is ready, ``False`` otherwise.
"""
with self.handle:
return self.__client.service_server_is_available()

def configure_introspection(
self, clock: Clock,
service_event_qos_profile: QoSProfile,
introspection_state: ServiceIntrospectionState
) -> None:
"""
Configure client introspection.

:param clock: Clock to use for generating timestamps.
:param service_event_qos_profile: QoSProfile to use when creating service event publisher.
:param introspection_state: ServiceIntrospectionState to set introspection.
"""
with self.handle:
self.__client.configure_introspection(clock.handle,
service_event_qos_profile.get_c_qos_profile(),
introspection_state)

@property
def handle(self) -> '_rclpy.Client[SrvRequestT, SrvResponseT]':
return self.__client

@property
def service_name(self) -> str:
with self.handle:
return self.__client.service_name

@property
def logger_name(self) -> str:
"""Get the name of the logger associated with the node of the client."""
with self.handle:
return self.__client.get_logger_name()

def destroy(self) -> None:
"""
Destroy a container for a ROS service client.

.. warning:: Users should not destroy a service client with this destructor, instead they
should call :meth:`.Node.destroy_client`.
"""
self.__client.destroy_when_not_in_use()

def __enter__(self) -> 'BaseClient[SrvRequestT, SrvResponseT]':
return self

def __exit__(
self,
exc_type: Optional[Type[BaseException]],
exc_val: Optional[BaseException],
exc_tb: Optional[TracebackType],
) -> None:
self.destroy()


class Client(BaseClient[SrvRequestT, SrvResponseT], Generic[SrvRequestT, SrvResponseT]):
def __init__(
self,
context: Context,
client_impl: '_rclpy.Client[SrvRequestT, SrvResponseT]',
srv_type: type[Srv[SrvRequestT, SrvResponseT]],
srv_name: str,
qos_profile: QoSProfile,
*,
callback_group: CallbackGroup
) -> None:
"""
Expand All @@ -60,11 +138,13 @@ def __init__(
:param callback_group: The callback group for the service client. If ``None``, then the
nodes default callback group is used.
"""
self.context = context
self.__client = client_impl
self.srv_type = srv_type
self.srv_name = srv_name
self.qos_profile = qos_profile
super().__init__(
context=context,
client_impl=client_impl,
srv_type=srv_type,
srv_name=srv_name,
qos_profile=qos_profile
)
# Key is a sequence number, value is an instance of a Future
self._pending_requests: Dict[int, Future[SrvResponseT]] = {}
self.callback_group = callback_group
Expand Down Expand Up @@ -132,7 +212,7 @@ def call_async(self, request: SrvRequestT) -> Future[SrvResponseT]:

with self._lock:
with self.handle:
sequence_number = self.__client.send_request(request)
sequence_number = self.handle.send_request(request)
if sequence_number in self._pending_requests:
raise RuntimeError(f'Sequence ({sequence_number}) conflicts with pending request')

Expand Down Expand Up @@ -168,15 +248,6 @@ def remove_pending_request(self, future: Future[SrvResponseT]) -> None:
del self._pending_requests[seq]
break

def service_is_ready(self) -> bool:
"""
Check if there is a service server ready.

:return: ``True`` if a server is ready, ``False`` otherwise.
"""
with self.handle:
return self.__client.service_server_is_available()

def wait_for_service(self, timeout_sec: Optional[float] = None) -> bool:
"""
Wait for a service server to become ready.
Expand All @@ -197,55 +268,3 @@ def wait_for_service(self, timeout_sec: Optional[float] = None) -> bool:
timeout_sec -= sleep_time

return self.service_is_ready()

def configure_introspection(
self, clock: Clock,
service_event_qos_profile: QoSProfile,
introspection_state: ServiceIntrospectionState
) -> None:
"""
Configure client introspection.

:param clock: Clock to use for generating timestamps.
:param service_event_qos_profile: QoSProfile to use when creating service event publisher.
:param introspection_state: ServiceIntrospectionState to set introspection.
"""
with self.handle:
self.__client.configure_introspection(clock.handle,
service_event_qos_profile.get_c_qos_profile(),
introspection_state)

@property
def handle(self) -> '_rclpy.Client[SrvRequestT, SrvResponseT]':
return self.__client

@property
def service_name(self) -> str:
with self.handle:
return self.__client.service_name

@property
def logger_name(self) -> str:
"""Get the name of the logger associated with the node of the client."""
with self.handle:
return self.__client.get_logger_name()

def destroy(self) -> None:
"""
Destroy a container for a ROS service client.

.. warning:: Users should not destroy a service client with this destructor, instead they
should call :meth:`.Node.destroy_client`.
"""
self.__client.destroy_when_not_in_use()

def __enter__(self) -> 'Client[SrvRequestT, SrvResponseT]':
return self

def __exit__(
self,
exc_type: Optional[Type[BaseException]],
exc_val: Optional[BaseException],
exc_tb: Optional[TracebackType],
) -> None:
self.destroy()
4 changes: 3 additions & 1 deletion rclpy/rclpy/executors.py
Original file line number Diff line number Diff line change
Expand Up @@ -722,7 +722,9 @@ def _wait_for_ready_callbacks(
timeout_nsec = timeout_sec_to_nsec(
timeout_sec.timeout if isinstance(timeout_sec, TimeoutObject) else timeout_sec)
if timeout_nsec > 0:
timeout_timer = Timer(None, None, timeout_nsec, self._clock, context=self._context)
timeout_timer = Timer(
None, timeout_nsec, self._clock, context=self._context
)

yielded_work = False
while not yielded_work and not self._is_shutdown and not condition():
Expand Down
20 changes: 10 additions & 10 deletions rclpy/rclpy/lifecycle/node.py
Original file line number Diff line number Diff line change
Expand Up @@ -143,36 +143,36 @@ def __init__(
lifecycle_msgs.srv.ChangeState,
self._state_machine.service_change_state.name,
self.__on_change_state,
callback_group,
QoSProfile(**self._state_machine.service_change_state.qos))
QoSProfile(**self._state_machine.service_change_state.qos),
callback_group=callback_group)
self._service_get_state = Service(
self._state_machine.service_get_state,
lifecycle_msgs.srv.GetState,
self._state_machine.service_get_state.name,
self.__on_get_state,
callback_group,
QoSProfile(**self._state_machine.service_get_state.qos))
QoSProfile(**self._state_machine.service_get_state.qos),
callback_group=callback_group)
self._service_get_available_states = Service(
self._state_machine.service_get_available_states,
lifecycle_msgs.srv.GetAvailableStates,
self._state_machine.service_get_available_states.name,
self.__on_get_available_states,
callback_group,
QoSProfile(**self._state_machine.service_get_available_states.qos))
QoSProfile(**self._state_machine.service_get_available_states.qos),
callback_group=callback_group)
self._service_get_available_transitions = Service(
self._state_machine.service_get_available_transitions,
lifecycle_msgs.srv.GetAvailableTransitions,
self._state_machine.service_get_available_transitions.name,
self.__on_get_available_transitions,
callback_group,
QoSProfile(**self._state_machine.service_get_available_transitions.qos))
QoSProfile(**self._state_machine.service_get_available_transitions.qos),
callback_group=callback_group)
self._service_get_transition_graph = Service(
self._state_machine.service_get_transition_graph,
lifecycle_msgs.srv.GetAvailableTransitions,
self._state_machine.service_get_transition_graph.name,
self.__on_get_transition_graph,
callback_group,
QoSProfile(**self._state_machine.service_get_transition_graph.qos))
QoSProfile(**self._state_machine.service_get_transition_graph.qos),
callback_group=callback_group)

lifecycle_services = [
self._service_change_state,
Expand Down
3 changes: 1 addition & 2 deletions rclpy/rclpy/lifecycle/publisher.py
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,7 @@
from .managed_entity import SimpleManagedEntity

if TYPE_CHECKING:
LifecyclePublisherArgs: TypeAlias = Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile,
PublisherEventCallbacks, CallbackGroup]
LifecyclePublisherArgs: TypeAlias = Tuple[_rclpy.Publisher[MsgT], Type[MsgT], str, QoSProfile]

class LifecyclePublisherKWArgs(TypedDict, Generic[MsgT]):
publisher_impl: _rclpy.Publisher[MsgT]
Expand Down
15 changes: 10 additions & 5 deletions rclpy/rclpy/node.py
Original file line number Diff line number Diff line change
Expand Up @@ -1730,7 +1730,8 @@ def create_subscription(
try:
subscription = Subscription(
subscription_object, msg_type,
topic, callback, callback_group, qos_profile, raw,
topic, callback, qos_profile, raw,
callback_group=callback_group,
event_callbacks=event_callbacks or SubscriptionEventCallbacks())
except Exception:
subscription_object.destroy_when_not_in_use()
Expand Down Expand Up @@ -1780,7 +1781,7 @@ def create_client(
client = Client(
self.context,
client_impl, srv_type, srv_name, qos_profile,
callback_group)
callback_group=callback_group)
callback_group.add_entity(client)
self._clients.append(client)
self._wake_executor()
Expand Down Expand Up @@ -1824,7 +1825,8 @@ def create_service(

service = Service(
service_impl,
srv_type, srv_name, callback, callback_group, qos_profile)
srv_type, srv_name, callback, qos_profile,
callback_group=callback_group)
callback_group.add_entity(service)
self._services.append(service)
self._wake_executor()
Expand Down Expand Up @@ -1860,8 +1862,11 @@ def create_timer(
if clock is None:
clock = self._clock
timer = Timer(
callback, callback_group, timer_period_nsec, clock, context=self.context,
autostart=autostart)
callback, timer_period_nsec, clock,
callback_group=callback_group,
context=self.context,
autostart=autostart
)

callback_group.add_entity(timer)
self._timers.append(timer)
Expand Down
Loading