diff --git a/rclpy/rclpy/__init__.py b/rclpy/rclpy/__init__.py index 38f6cd406..8b7db10b0 100644 --- a/rclpy/rclpy/__init__.py +++ b/rclpy/rclpy/__init__.py @@ -42,9 +42,12 @@ import sys from typing import List -from typing import TYPE_CHECKING from rclpy.context import Context +from rclpy.executors import Executor +from rclpy.executors import SingleThreadedExecutor +from rclpy.impl.implementation_singleton import get_rclpy_implementation +from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.task import Future from rclpy.utilities import get_default_context @@ -53,11 +56,6 @@ from rclpy.utilities import shutdown as _shutdown from rclpy.utilities import try_shutdown # noqa: F401 -# Avoid loading extensions on module import -if TYPE_CHECKING: - from rclpy.executors import Executor # noqa: F401 - from rclpy.node import Node # noqa: F401 - def init(*, args: List[str] = None, context: Context = None) -> None: """ @@ -68,9 +66,9 @@ def init(*, args: List[str] = None, context: Context = None) -> None: (see :func:`.get_default_context`). """ context = get_default_context() if context is None else context - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - return rclpy_implementation.rclpy_init(args if args is not None else sys.argv, context.handle) + return get_rclpy_implementation().rclpy_init( + args if args is not None else sys.argv, context.handle + ) # The global spin functions need an executor to do the work @@ -78,11 +76,9 @@ def init(*, args: List[str] = None, context: Context = None) -> None: __executor = None -def get_global_executor() -> 'Executor': +def get_global_executor() -> Executor: global __executor if __executor is None: - # imported locally to avoid loading extensions on module import - from rclpy.executors import SingleThreadedExecutor __executor = SingleThreadedExecutor() return __executor @@ -114,7 +110,7 @@ def create_node( parameter_overrides: List[Parameter] = None, allow_undeclared_parameters: bool = False, automatically_declare_parameters_from_overrides: bool = False -) -> 'Node': +) -> Node: """ Create an instance of :class:`.Node`. @@ -136,8 +132,6 @@ def create_node( be used to implicitly declare parameters on the node during creation, default False. :return: An instance of the newly created node. """ - # imported locally to avoid loading extensions on module import - from rclpy.node import Node # noqa: F811 return Node( node_name, context=context, cli_args=cli_args, namespace=namespace, use_global_arguments=use_global_arguments, diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index fad5ddb3d..9ef4e76f9 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -21,9 +21,8 @@ from action_msgs.srv import CancelGoal from rclpy.executors import await_or_execute -from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action -from rclpy.qos import qos_profile_action_status_default -from rclpy.qos import qos_profile_services_default +from rclpy.impl.implementation_singleton import get_rclpy_action_implementation +from rclpy.qos import qos_profiles from rclpy.qos import QoSProfile from rclpy.task import Future from rclpy.type_support import check_for_type_support @@ -118,11 +117,11 @@ def __init__( action_name, *, 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 + goal_service_qos_profile=None, + result_service_qos_profile=None, + cancel_service_qos_profile=None, + feedback_sub_qos_profile=None, + status_sub_qos_profile=None ): """ Constructor. @@ -139,6 +138,17 @@ def __init__( :param feedback_sub_qos_profile: QoS profile for the feedback subscriber. :param status_sub_qos_profile: QoS profile for the status subscriber. """ + if goal_service_qos_profile is None: + goal_service_qos_profile = qos_profiles.services_default + if result_service_qos_profile is None: + result_service_qos_profile = qos_profiles.services_default + if cancel_service_qos_profile is None: + cancel_service_qos_profile = qos_profiles.services_default + if feedback_sub_qos_profile is None: + feedback_sub_qos_profile = QoSProfile(depth=10) + if status_sub_qos_profile is None: + status_sub_qos_profile = qos_profiles.action_status_default + if callback_group is None: callback_group = node.default_callback_group @@ -149,7 +159,7 @@ def __init__( self._node = node self._action_type = action_type with node.handle as node_capsule: - self._client_handle = _rclpy_action.rclpy_action_create_client( + self._client_handle = get_rclpy_action_implementation().rclpy_action_create_client( node_capsule, action_type, action_name, @@ -219,7 +229,7 @@ def _remove_pending_result_request(self, future): # Start Waitable API def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" - ready_entities = _rclpy_action.rclpy_action_wait_set_is_ready( + ready_entities = get_rclpy_action_implementation().rclpy_action_wait_set_is_ready( self._client_handle, wait_set) self._is_feedback_ready = ready_entities[0] @@ -233,35 +243,35 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" data = {} if self._is_goal_response_ready: - taken_data = _rclpy_action.rclpy_action_take_goal_response( + taken_data = get_rclpy_action_implementation().rclpy_action_take_goal_response( self._client_handle, self._action_type.Impl.SendGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['goal'] = taken_data if self._is_cancel_response_ready: - taken_data = _rclpy_action.rclpy_action_take_cancel_response( + taken_data = get_rclpy_action_implementation().rclpy_action_take_cancel_response( self._client_handle, self._action_type.Impl.CancelGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['cancel'] = taken_data if self._is_result_response_ready: - taken_data = _rclpy_action.rclpy_action_take_result_response( + taken_data = get_rclpy_action_implementation().rclpy_action_take_result_response( self._client_handle, self._action_type.Impl.GetResultService.Response) # If take fails, then we get (None, None) if all(taken_data): data['result'] = taken_data if self._is_feedback_ready: - taken_data = _rclpy_action.rclpy_action_take_feedback( + taken_data = get_rclpy_action_implementation().rclpy_action_take_feedback( self._client_handle, self._action_type.Impl.FeedbackMessage) # If take fails, then we get None if taken_data is not None: data['feedback'] = taken_data if self._is_status_ready: - taken_data = _rclpy_action.rclpy_action_take_status( + taken_data = get_rclpy_action_implementation().rclpy_action_take_status( self._client_handle, self._action_type.Impl.GoalStatusMessage) # If take fails, then we get None if taken_data is not None: @@ -328,12 +338,17 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = _rclpy_action.rclpy_action_wait_set_get_num_entities(self._client_handle) + num_entities = get_rclpy_action_implementation().rclpy_action_wait_set_get_num_entities( + self._client_handle + ) return NumberOfEntities(*num_entities) def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - _rclpy_action.rclpy_action_wait_set_add(self._client_handle, wait_set) + get_rclpy_action_implementation().rclpy_action_wait_set_add( + self._client_handle, + wait_set + ) # End Waitable API def send_goal(self, goal, **kwargs): @@ -404,7 +419,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): request = self._action_type.Impl.SendGoalService.Request() request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid request.goal = goal - sequence_number = _rclpy_action.rclpy_action_send_goal_request( + sequence_number = get_rclpy_action_implementation().rclpy_action_send_goal_request( self._client_handle, request) if sequence_number in self._pending_goal_requests: raise RuntimeError( @@ -463,7 +478,7 @@ def _cancel_goal_async(self, goal_handle): cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = _rclpy_action.rclpy_action_send_cancel_request( + sequence_number = get_rclpy_action_implementation().rclpy_action_send_cancel_request( self._client_handle, cancel_request) if sequence_number in self._pending_cancel_requests: @@ -517,7 +532,7 @@ def _get_result_async(self, goal_handle): result_request = self._action_type.Impl.GetResultService.Request() result_request.goal_id = goal_handle.goal_id - sequence_number = _rclpy_action.rclpy_action_send_result_request( + sequence_number = get_rclpy_action_implementation().rclpy_action_send_result_request( self._client_handle, result_request) if sequence_number in self._pending_result_requests: @@ -539,7 +554,7 @@ def server_is_ready(self): :return: True if an action server is ready, False otherwise. """ with self._node.handle as node_capsule: - return _rclpy_action.rclpy_action_server_is_available( + return get_rclpy_action_implementation().rclpy_action_server_is_available( node_capsule, self._client_handle) @@ -568,7 +583,10 @@ def destroy(self): if self._client_handle is None: return with self._node.handle as node_capsule: - _rclpy_action.rclpy_action_destroy_entity(self._client_handle, node_capsule) + get_rclpy_action_implementation().rclpy_action_destroy_entity( + self._client_handle, + node_capsule + ) self._node.remove_waitable(self) self._client_handle = None diff --git a/rclpy/rclpy/action/graph.py b/rclpy/rclpy/action/graph.py index 864312c6b..6585d2e10 100644 --- a/rclpy/rclpy/action/graph.py +++ b/rclpy/rclpy/action/graph.py @@ -15,7 +15,7 @@ from typing import List from typing import Tuple -from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action +from rclpy.impl.implementation_singleton import get_rclpy_action_implementation from rclpy.node import Node @@ -35,7 +35,7 @@ def get_action_client_names_and_types_by_node( action types. """ with node.handle as node_capsule: - return _rclpy_action.rclpy_action_get_client_names_and_types_by_node( + return get_rclpy_action_implementation().rclpy_action_get_client_names_and_types_by_node( node_capsule, remote_node_name, remote_node_namespace) @@ -55,7 +55,7 @@ def get_action_server_names_and_types_by_node( action types. """ with node.handle as node_capsule: - return _rclpy_action.rclpy_action_get_server_names_and_types_by_node( + return get_rclpy_action_implementation().rclpy_action_get_server_names_and_types_by_node( node_capsule, remote_node_name, remote_node_namespace) @@ -69,4 +69,4 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: action types. """ with node.handle as node_capsule: - return _rclpy_action.rclpy_action_get_names_and_types(node_capsule) + return get_rclpy_action_implementation().rclpy_action_get_names_and_types(node_capsule) diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 8e86546a8..f113ca086 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -19,9 +19,8 @@ from action_msgs.msg import GoalInfo, GoalStatus from rclpy.executors import await_or_execute -from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action -from rclpy.qos import qos_profile_action_status_default -from rclpy.qos import qos_profile_services_default +from rclpy.impl.implementation_singleton import get_rclpy_action_implementation +from rclpy.qos import qos_profiles from rclpy.qos import QoSProfile from rclpy.task import Future from rclpy.type_support import check_for_type_support @@ -68,7 +67,7 @@ def __init__(self, action_server, goal_info, goal_request): :param goal_info: GoalInfo message. :param goal_request: The user defined goal request message from an ActionClient. """ - self._handle = _rclpy_action.rclpy_action_accept_new_goal( + self._handle = get_rclpy_action_implementation().rclpy_action_accept_new_goal( action_server._handle, goal_info) self._action_server = action_server self._goal_info = goal_info @@ -97,7 +96,9 @@ def is_active(self): with self._lock: if self._handle is None: return False - return _rclpy_action.rclpy_action_goal_handle_is_active(self._handle) + return get_rclpy_action_implementation().rclpy_action_goal_handle_is_active( + self._handle + ) @property def is_cancel_requested(self): @@ -108,7 +109,9 @@ def status(self): with self._lock: if self._handle is None: return GoalStatus.STATUS_UNKNOWN - return _rclpy_action.rclpy_action_goal_handle_get_status(self._handle) + return get_rclpy_action_implementation().rclpy_action_goal_handle_get_status( + self._handle + ) def _update_state(self, event): with self._lock: @@ -117,13 +120,20 @@ def _update_state(self, event): return # Update state - _rclpy_action.rclpy_action_update_goal_state(self._handle, event.value) + get_rclpy_action_implementation().rclpy_action_update_goal_state( + self._handle, + event.value + ) # Publish state change - _rclpy_action.rclpy_action_publish_status(self._action_server._handle) + get_rclpy_action_implementation().rclpy_action_publish_status( + self._action_server._handle + ) # If it's a terminal state, then also notify the action server - if not _rclpy_action.rclpy_action_goal_handle_is_active(self._handle): + if not get_rclpy_action_implementation().rclpy_action_goal_handle_is_active( + self._handle + ): self._action_server.notify_goal_done() def execute(self, execute_callback=None): @@ -150,7 +160,7 @@ def publish_feedback(self, feedback): feedback_message.feedback = feedback # Publish - _rclpy_action.rclpy_action_publish_feedback( + get_rclpy_action_implementation().rclpy_action_publish_feedback( self._action_server._handle, feedback_message) def succeed(self): @@ -166,7 +176,7 @@ def destroy(self): with self._lock: if self._handle is None: return - _rclpy_action.rclpy_action_destroy_server_goal_handle(self._handle) + get_rclpy_action_implementation().rclpy_action_destroy_server_goal_handle(self._handle) self._handle = None self._action_server.remove_future(self._result_future) @@ -201,11 +211,11 @@ def __init__( 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, + goal_service_qos_profile=None, + result_service_qos_profile=None, + cancel_service_qos_profile=None, + feedback_pub_qos_profile=None, + status_pub_qos_profile=None, result_timeout=900 ): """ @@ -232,6 +242,17 @@ def __init__( :param result_timeout: How long in seconds a result is kept by the server after a goal reaches a terminal state. """ + if goal_service_qos_profile is None: + goal_service_qos_profile = qos_profiles.services_default + if result_service_qos_profile is None: + result_service_qos_profile = qos_profiles.services_default + if cancel_service_qos_profile is None: + cancel_service_qos_profile = qos_profiles.services_default + if feedback_pub_qos_profile is None: + feedback_pub_qos_profile = QoSProfile(depth=10) + if status_pub_qos_profile is None: + status_pub_qos_profile = qos_profiles.action_status_default + if callback_group is None: callback_group = node.default_callback_group @@ -249,7 +270,7 @@ def __init__( self._node = node self._action_type = action_type with node.handle as node_capsule, node.get_clock().handle as clock_capsule: - self._handle = _rclpy_action.rclpy_action_create_server( + self._handle = get_rclpy_action_implementation().rclpy_action_create_server( node_capsule, clock_capsule, action_type, @@ -278,7 +299,10 @@ async def _execute_goal_request(self, request_header_and_message): # Check if goal ID is already being tracked by this action server with self._lock: - goal_id_exists = _rclpy_action.rclpy_action_server_goal_exists(self._handle, goal_info) + goal_id_exists = get_rclpy_action_implementation().rclpy_action_server_goal_exists( + self._handle, + goal_info + ) accepted = False if not goal_id_exists: @@ -309,7 +333,7 @@ async def _execute_goal_request(self, request_header_and_message): response_msg = self._action_type.Impl.SendGoalService.Response() response_msg.accepted = accepted response_msg.stamp = goal_info.stamp - _rclpy_action.rclpy_action_send_goal_response( + get_rclpy_action_implementation().rclpy_action_send_goal_response( self._handle, request_header, response_msg, @@ -353,8 +377,12 @@ async def _execute_cancel_request(self, request_header_and_message): with self._lock: # Get list of goals that are requested to be canceled - cancel_response = _rclpy_action.rclpy_action_process_cancel_request( - self._handle, cancel_request, self._action_type.Impl.CancelGoalService.Response) + cancel_response = get_rclpy_action_implementation( + ).rclpy_action_process_cancel_request( + self._handle, + cancel_request, + self._action_type.Impl.CancelGoalService.Response, + ) for goal_info in cancel_response.goals_canceling: goal_uuid = bytes(goal_info.goal_id.uuid) @@ -373,7 +401,7 @@ async def _execute_cancel_request(self, request_header_and_message): # Remove from response cancel_response.goals_canceling.remove(goal_info) - _rclpy_action.rclpy_action_send_cancel_response( + get_rclpy_action_implementation().rclpy_action_send_cancel_response( self._handle, request_header, cancel_response, @@ -392,7 +420,7 @@ async def _execute_get_result_request(self, request_header_and_message): 'Sending result response for unknown goal ID: {0}'.format(goal_uuid)) result_response = self._action_type.Impl.GetResultService.Response() result_response.status = GoalStatus.STATUS_UNKNOWN - _rclpy_action.rclpy_action_send_result_response( + get_rclpy_action_implementation().rclpy_action_send_result_response( self._handle, request_header, result_response, @@ -410,7 +438,7 @@ async def _execute_expire_goals(self, expired_goals): del self._goal_handles[goal_uuid] def _send_result_response(self, request_header, future): - _rclpy_action.rclpy_action_send_result_response( + get_rclpy_action_implementation().rclpy_action_send_result_response( self._handle, request_header, future.result(), @@ -424,7 +452,10 @@ def action_type(self): def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" with self._lock: - ready_entities = _rclpy_action.rclpy_action_wait_set_is_ready(self._handle, wait_set) + ready_entities = get_rclpy_action_implementation().rclpy_action_wait_set_is_ready( + self._handle, + wait_set + ) self._is_goal_request_ready = ready_entities[0] self._is_cancel_request_ready = ready_entities[1] self._is_result_request_ready = ready_entities[2] @@ -436,7 +467,7 @@ def take_data(self): data = {} if self._is_goal_request_ready: with self._lock: - taken_data = _rclpy_action.rclpy_action_take_goal_request( + taken_data = get_rclpy_action_implementation().rclpy_action_take_goal_request( self._handle, self._action_type.Impl.SendGoalService.Request, ) @@ -446,7 +477,7 @@ def take_data(self): if self._is_cancel_request_ready: with self._lock: - taken_data = _rclpy_action.rclpy_action_take_cancel_request( + taken_data = get_rclpy_action_implementation().rclpy_action_take_cancel_request( self._handle, self._action_type.Impl.CancelGoalService.Request, ) @@ -456,7 +487,7 @@ def take_data(self): if self._is_result_request_ready: with self._lock: - taken_data = _rclpy_action.rclpy_action_take_result_request( + taken_data = get_rclpy_action_implementation().rclpy_action_take_result_request( self._handle, self._action_type.Impl.GetResultService.Request, ) @@ -466,7 +497,7 @@ def take_data(self): if self._is_goal_expired: with self._lock: - data['expired'] = _rclpy_action.rclpy_action_expire_goals( + data['expired'] = get_rclpy_action_implementation().rclpy_action_expire_goals( self._handle, len(self._goal_handles), ) @@ -494,7 +525,9 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = _rclpy_action.rclpy_action_wait_set_get_num_entities(self._handle) + num_entities = get_rclpy_action_implementation().rclpy_action_wait_set_get_num_entities( + self._handle + ) return NumberOfEntities( num_entities[0], num_entities[1], @@ -505,7 +538,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" with self._lock: - _rclpy_action.rclpy_action_wait_set_add(self._handle, wait_set) + get_rclpy_action_implementation().rclpy_action_wait_set_add(self._handle, wait_set) # End Waitable API def notify_execute(self, goal_handle, execute_callback): @@ -520,7 +553,7 @@ def notify_execute(self, goal_handle, execute_callback): def notify_goal_done(self): with self._lock: - _rclpy_action.rclpy_action_notify_goal_done(self._handle) + get_rclpy_action_implementation().rclpy_action_notify_goal_done(self._handle) def register_handle_accepted_callback(self, handle_accepted_callback): """ @@ -609,7 +642,10 @@ def destroy(self): goal_handle.destroy() with self._node.handle as node_capsule: - _rclpy_action.rclpy_action_destroy_entity(self._handle, node_capsule) + get_rclpy_action_implementation().rclpy_action_destroy_entity( + self._handle, + node_capsule + ) self._node.remove_waitable(self) self._handle = None diff --git a/rclpy/rclpy/client.py b/rclpy/rclpy/client.py index bf06e2dd3..decd6b1c3 100644 --- a/rclpy/rclpy/client.py +++ b/rclpy/rclpy/client.py @@ -19,7 +19,7 @@ from rclpy.callback_groups import CallbackGroup from rclpy.context import Context -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.qos import QoSProfile from rclpy.task import Future @@ -123,7 +123,7 @@ def call_async(self, request: SrvTypeRequest) -> Future: raise TypeError() with self.handle as capsule: - sequence_number = _rclpy.rclpy_send_request(capsule, request) + sequence_number = get_rclpy_implementation().rclpy_send_request(capsule, request) if sequence_number in self._pending_requests: raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number) @@ -141,7 +141,7 @@ def service_is_ready(self) -> bool: :return: ``True`` if a server is ready, ``False`` otherwise. """ with self.handle as capsule: - return _rclpy.rclpy_service_server_is_available(capsule) + return get_rclpy_implementation().rclpy_service_server_is_available(capsule) def wait_for_service(self, timeout_sec: float = None) -> bool: """ diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index a58229891..659701faf 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -15,7 +15,7 @@ from enum import IntEnum from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from .duration import Duration @@ -107,14 +107,14 @@ def __init__(self, *, clock, threshold, pre_callback, post_callback): min_backward = threshold.min_backward.nanoseconds with self._clock.handle as clock_capsule: - _rclpy.rclpy_add_clock_callback( + get_rclpy_implementation().rclpy_add_clock_callback( clock_capsule, self, threshold.on_clock_change, min_forward, min_backward) def unregister(self): """Remove a jump callback from the clock.""" if self._clock is not None: with self._clock.handle as clock_capsule: - _rclpy.rclpy_remove_clock_callback(clock_capsule, self) + get_rclpy_implementation().rclpy_remove_clock_callback(clock_capsule, self) self._clock = None @@ -127,7 +127,7 @@ def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): self = super().__new__(ROSClock) else: self = super().__new__(cls) - self.__handle = Handle(_rclpy.rclpy_create_clock(clock_type)) + self.__handle = Handle(get_rclpy_implementation().rclpy_create_clock(clock_type)) self._clock_type = clock_type return self @@ -145,10 +145,10 @@ def __repr__(self): def now(self): from rclpy.time import Time with self.handle as clock_capsule: - time_handle = _rclpy.rclpy_clock_get_now(clock_capsule) + time_handle = get_rclpy_implementation().rclpy_clock_get_now(clock_capsule) # TODO(dhood): Return a python object from the C extension return Time( - nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle), + nanoseconds=get_rclpy_implementation().rclpy_time_point_get_nanoseconds(time_handle), clock_type=self.clock_type) def create_jump_callback(self, threshold, *, pre_callback=None, post_callback=None): @@ -198,12 +198,17 @@ def __new__(cls): @property def ros_time_is_active(self): with self.handle as clock_capsule: - return _rclpy.rclpy_clock_get_ros_time_override_is_enabled(clock_capsule) + return get_rclpy_implementation().rclpy_clock_get_ros_time_override_is_enabled( + clock_capsule + ) def _set_ros_time_is_active(self, enabled): # This is not public because it is only to be called by a TimeSource managing the Clock with self.handle as clock_capsule: - _rclpy.rclpy_clock_set_ros_time_override_is_enabled(clock_capsule, enabled) + get_rclpy_implementation().rclpy_clock_set_ros_time_override_is_enabled( + clock_capsule, + enabled + ) def set_ros_time_override(self, time): from rclpy.time import Time @@ -211,4 +216,7 @@ def set_ros_time_override(self, time): raise TypeError( 'Time must be specified as rclpy.time.Time. Received type: {0}'.format(type(time))) with self.handle as clock_capsule: - _rclpy.rclpy_clock_set_ros_time_override(clock_capsule, time._time_handle) + get_rclpy_implementation().rclpy_clock_set_ros_time_override( + clock_capsule, + time._time_handle + ) diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index ac628572c..34d281ede 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -15,6 +15,8 @@ import threading from typing import Callable +from rclpy.impl.implementation_singleton import get_rclpy_implementation + class Context: """ @@ -26,8 +28,7 @@ class Context: """ def __init__(self): - from rclpy.impl.implementation_singleton import rclpy_implementation - self._handle = rclpy_implementation.rclpy_create_context() + self._handle = get_rclpy_implementation().rclpy_create_context() self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() @@ -38,10 +39,8 @@ def handle(self): def ok(self): """Check if context hasn't been shut down.""" - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation with self._lock: - return rclpy_implementation.rclpy_ok(self._handle) + return get_rclpy_implementation().rclpy_ok(self._handle) def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -51,19 +50,15 @@ def _call_on_shutdown_callbacks(self): def shutdown(self): """Shutdown this context.""" - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation with self._lock: - rclpy_implementation.rclpy_shutdown(self._handle) + get_rclpy_implementation().rclpy_shutdown(self._handle) self._call_on_shutdown_callbacks() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation with self._lock: - if rclpy_implementation.rclpy_ok(self._handle): - rclpy_implementation.rclpy_shutdown(self._handle) + if get_rclpy_implementation().rclpy_ok(self._handle): + get_rclpy_implementation().rclpy_shutdown(self._handle) self._call_on_shutdown_callbacks() def on_shutdown(self, callback: Callable[[], None]): diff --git a/rclpy/rclpy/duration.py b/rclpy/rclpy/duration.py index d84ad932c..9c5aff39c 100644 --- a/rclpy/rclpy/duration.py +++ b/rclpy/rclpy/duration.py @@ -13,7 +13,7 @@ # limitations under the License. import builtin_interfaces -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation class Duration: @@ -22,14 +22,18 @@ def __init__(self, *, seconds=0, nanoseconds=0): total_nanoseconds = int(seconds * 1e9) total_nanoseconds += int(nanoseconds) try: - self._duration_handle = _rclpy.rclpy_create_duration(total_nanoseconds) + self._duration_handle = get_rclpy_implementation().rclpy_create_duration( + total_nanoseconds + ) except OverflowError as e: raise OverflowError( 'Total nanoseconds value is too large to store in C time point.') from e @property def nanoseconds(self): - return _rclpy.rclpy_duration_get_nanoseconds(self._duration_handle) + return get_rclpy_implementation().rclpy_duration_get_nanoseconds( + self._duration_handle + ) def __repr__(self): return 'Duration(nanoseconds={0})'.format(self.nanoseconds) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 0aa6d1824..f4548dffb 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -37,7 +37,7 @@ from rclpy.context import Context from rclpy.guard_condition import GuardCondition from rclpy.handle import InvalidHandle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.service import Service from rclpy.signals import SignalHandlerGuardCondition from rclpy.subscription import Subscription @@ -62,11 +62,11 @@ class _WaitSet: """Make sure the wait set gets destroyed when a generator exits.""" def __enter__(self): - self.wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() + self.wait_set = get_rclpy_implementation().rclpy_get_zero_initialized_wait_set() return self.wait_set def __exit__(self, t, v, tb): - _rclpy.rclpy_destroy_wait_set(self.wait_set) + get_rclpy_implementation().rclpy_destroy_wait_set(self.wait_set) class _WorkTracker: @@ -311,14 +311,14 @@ def spin_once(self, timeout_sec: float = None) -> None: def _take_timer(self, tmr): with tmr.handle as capsule: - _rclpy.rclpy_call_timer(capsule) + get_rclpy_implementation().rclpy_call_timer(capsule) async def _execute_timer(self, tmr, _): await await_or_execute(tmr.callback) def _take_subscription(self, sub): with sub.handle as capsule: - msg = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw) + msg = get_rclpy_implementation().rclpy_take(capsule, sub.msg_type, sub.raw) return msg async def _execute_subscription(self, sub, msg): @@ -327,7 +327,10 @@ async def _execute_subscription(self, sub, msg): def _take_client(self, client): with client.handle as capsule: - return _rclpy.rclpy_take_response(capsule, client.srv_type.Response) + return get_rclpy_implementation().rclpy_take_response( + capsule, + client.srv_type.Response + ) async def _execute_client(self, client, seq_and_response): sequence, response = seq_and_response @@ -343,7 +346,10 @@ async def _execute_client(self, client, seq_and_response): def _take_service(self, srv): with srv.handle as capsule: - request_and_header = _rclpy.rclpy_take_request(capsule, srv.srv_type.Request) + request_and_header = get_rclpy_implementation().rclpy_take_request( + capsule, + srv.srv_type.Request + ) return request_and_header async def _execute_service(self, srv, request_and_header): @@ -528,7 +534,7 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_guard_conditions -= 1 - _rclpy.rclpy_wait_set_init( + get_rclpy_implementation().rclpy_wait_set_init( wait_set, entity_count.num_subscriptions, entity_count.num_guard_conditions, @@ -538,33 +544,68 @@ def _wait_for_ready_callbacks( entity_count.num_events, self._context.handle) - _rclpy.rclpy_wait_set_clear_entities(wait_set) + get_rclpy_implementation().rclpy_wait_set_clear_entities(wait_set) for sub_capsule in sub_capsules: - _rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule) + get_rclpy_implementation().rclpy_wait_set_add_entity( + 'subscription', + wait_set, + sub_capsule + ) for cli_capsule in client_capsules: - _rclpy.rclpy_wait_set_add_entity('client', wait_set, cli_capsule) + get_rclpy_implementation().rclpy_wait_set_add_entity( + 'client', + wait_set, + cli_capsule + ) for srv_capsule in service_capsules: - _rclpy.rclpy_wait_set_add_entity('service', wait_set, srv_capsule) + get_rclpy_implementation().rclpy_wait_set_add_entity( + 'service', + wait_set, + srv_capsule + ) for tmr_capsule in timer_capsules: - _rclpy.rclpy_wait_set_add_entity('timer', wait_set, tmr_capsule) + get_rclpy_implementation().rclpy_wait_set_add_entity( + 'timer', + wait_set, + tmr_capsule + ) for gc_capsule in guard_capsules: - _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc_capsule) + get_rclpy_implementation().rclpy_wait_set_add_entity( + 'guard_condition', + wait_set, + gc_capsule + ) for waitable in waitables: waitable.add_to_wait_set(wait_set) # Wait for something to become ready - _rclpy.rclpy_wait(wait_set, timeout_nsec) + get_rclpy_implementation().rclpy_wait(wait_set, timeout_nsec) if self._is_shutdown: raise ShutdownException() if not self._context.ok(): raise ExternalShutdownException() # get ready entities - subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set) - guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set) - timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set) - clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set) - services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set) + subs_ready = get_rclpy_implementation().rclpy_get_ready_entities( + 'subscription', + wait_set + ) + guards_ready = get_rclpy_implementation().rclpy_get_ready_entities( + 'guard_condition', + wait_set + ) + timers_ready = get_rclpy_implementation().rclpy_get_ready_entities( + 'timer', + wait_set + ) + clients_ready = get_rclpy_implementation().rclpy_get_ready_entities( + 'client', + wait_set + ) + services_ready = get_rclpy_implementation().rclpy_get_ready_entities( + 'service', + wait_set + ) # Mark all guards as triggered before yielding since they're auto-taken for gc in guards: @@ -587,7 +628,7 @@ def _wait_for_ready_callbacks( if tmr.handle.pointer in timers_ready: with tmr.handle as capsule: # Check timer is ready to workaround rcl issue with cancelled timers - if _rclpy.rclpy_is_timer_ready(capsule): + if get_rclpy_implementation().rclpy_is_timer_ready(capsule): if tmr.callback_group.can_execute(tmr): handler = self._make_handler( tmr, node, self._take_timer, self._execute_timer) diff --git a/rclpy/rclpy/expand_topic_name.py b/rclpy/rclpy/expand_topic_name.py index 8d8130bab..c5a1b9f86 100644 --- a/rclpy/rclpy/expand_topic_name.py +++ b/rclpy/rclpy/expand_topic_name.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation def expand_topic_name(topic_name, node_name, node_namespace): @@ -30,4 +30,8 @@ def expand_topic_name(topic_name, node_name, node_namespace): :returns: expanded topic name which is fully qualified :raises: ValueError if the topic name, node name or namespace are invalid """ - return _rclpy.rclpy_expand_topic_name(topic_name, node_name, node_namespace) + return get_rclpy_implementation().rclpy_expand_topic_name( + topic_name, + node_name, + node_namespace + ) diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index d7b0d5487..a4347cd89 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -13,7 +13,7 @@ # limitations under the License. from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.utilities import get_default_context @@ -21,7 +21,9 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context - self.__handle = Handle(_rclpy.rclpy_create_guard_condition(self._context.handle)) + self.__handle = Handle(get_rclpy_implementation().rclpy_create_guard_condition( + 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 @@ -31,7 +33,7 @@ def __init__(self, callback, callback_group, context=None): def trigger(self): with self.handle as capsule: - _rclpy.rclpy_trigger_guard_condition(capsule) + get_rclpy_implementation().rclpy_trigger_guard_condition(capsule) @property def handle(self): diff --git a/rclpy/rclpy/handle.py b/rclpy/rclpy/handle.py index e252fe328..5720b9803 100644 --- a/rclpy/rclpy/handle.py +++ b/rclpy/rclpy/handle.py @@ -15,7 +15,7 @@ from threading import RLock import weakref -from rclpy.impl.implementation_singleton import rclpy_pycapsule_implementation as _rclpy_capsule +from rclpy.impl.implementation_singleton import get_rclpy_pycapsule_implementation class InvalidHandle(Exception): @@ -52,8 +52,10 @@ def __init__(self, pycapsule): self.__dependent_handles = weakref.WeakSet() self.__destroy_callbacks = [] # Called to give an opportunity to raise an exception if the object is not a pycapsule. - self.__capsule_name = _rclpy_capsule.rclpy_pycapsule_name(pycapsule) - self.__capsule_pointer = _rclpy_capsule.rclpy_pycapsule_pointer(pycapsule) + self.__capsule_name = get_rclpy_pycapsule_implementation().rclpy_pycapsule_name(pycapsule) + self.__capsule_pointer = get_rclpy_pycapsule_implementation().rclpy_pycapsule_pointer( + pycapsule + ) def __eq__(self, other): return self.__capsule_pointer == other.__capsule_pointer @@ -188,7 +190,7 @@ def remove_dependent(handle): def __destroy_self(self): with self.__rlock: # Calls pycapsule destructor - _rclpy_capsule.rclpy_pycapsule_destroy(self.__capsule) + get_rclpy_pycapsule_implementation().rclpy_pycapsule_destroy(self.__capsule) # Call post-destroy callbacks while self.__destroy_callbacks: self.__destroy_callbacks.pop()(self) diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py index bff893961..01bdbed46 100644 --- a/rclpy/rclpy/impl/implementation_singleton.py +++ b/rclpy/rclpy/impl/implementation_singleton.py @@ -1,4 +1,4 @@ -# Copyright 2016-2017 Open Source Robotics Foundation, Inc. +# Copyright 2016-2019 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. @@ -19,17 +19,59 @@ .. code:: - from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy + from rclpy.impl.implementation_singleton import get_rclpy_implementation - _rclpy.rclpy_init() - while _rclpy.rclpy_ok(): + get_rclpy_implementation().rclpy_init() + while get_rclpy_implementation().rclpy_ok(): # ... """ -from rclpy.impl import _import -rclpy_implementation = _import('._rclpy') -rclpy_action_implementation = _import('._rclpy_action') -rclpy_logging_implementation = _import('._rclpy_logging') -rclpy_signal_handler_implementation = _import('._rclpy_signal_handler') -rclpy_pycapsule_implementation = _import('._rclpy_pycapsule') +def get_rclpy_implementation(): + """ + Get the rclpy C extension. + + This function shouldn't be called from module level. + """ + from rclpy.impl import _import + return _import('._rclpy') + + +def get_rclpy_action_implementation(): + """ + Get the rclpy action C extension. + + This function shouldn't be called from module level. + """ + from rclpy.impl import _import + return _import('._rclpy_action') + + +def get_rclpy_logging_implementation(): + """ + Get the rclpy logging C extension. + + This function shouldn't be called from module level. + """ + from rclpy.impl import _import + return _import('._rclpy_logging') + + +def get_rclpy_signal_handler_implementation(): + """ + Get the rclpy C extension. + + This function shouldn't be called from module level. + """ + from rclpy.impl import _import + return _import('._rclpy_signal_handler') + + +def get_rclpy_pycapsule_implementation(): + """ + Get the rclpy pycapsule C extension. + + This function shouldn't be called from module level. + """ + from rclpy.impl import _import + return _import('._rclpy_pycapsule') diff --git a/rclpy/rclpy/impl/rcutils_logger.py b/rclpy/rclpy/impl/rcutils_logger.py index c1f394eb6..f321d791a 100644 --- a/rclpy/rclpy/impl/rcutils_logger.py +++ b/rclpy/rclpy/impl/rcutils_logger.py @@ -1,4 +1,4 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. +# Copyright 2017-2019 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. @@ -19,7 +19,7 @@ import os import time -from rclpy.impl.implementation_singleton import rclpy_logging_implementation as _rclpy_logging +from rclpy.impl.implementation_singleton import get_rclpy_logging_implementation # Known filenames from which logging methods can be called (will be ignored in `_find_caller`). _internal_callers = [] @@ -231,18 +231,24 @@ def get_child(self, name): def set_level(self, level): from rclpy.logging import LoggingSeverity level = LoggingSeverity(level) - return _rclpy_logging.rclpy_logging_set_logger_level(self.name, level) + return get_rclpy_logging_implementation().rclpy_logging_set_logger_level(self.name, level) def get_effective_level(self): from rclpy.logging import LoggingSeverity level = LoggingSeverity( - _rclpy_logging.rclpy_logging_get_logger_effective_level(self.name)) + get_rclpy_logging_implementation().rclpy_logging_get_logger_effective_level( + self.name + ) + ) return level def is_enabled_for(self, severity): from rclpy.logging import LoggingSeverity severity = LoggingSeverity(severity) - return _rclpy_logging.rclpy_logging_logger_is_enabled_for(self.name, severity) + return get_rclpy_logging_implementation().rclpy_logging_logger_is_enabled_for( + self.name, + severity + ) def log(self, message, severity, **kwargs): r""" @@ -322,7 +328,7 @@ def log(self, message, severity, **kwargs): return False # Call the relevant function from the C extension. - _rclpy_logging.rclpy_logging_rcutils_log( + get_rclpy_logging_implementation().rclpy_logging_rcutils_log( severity, name, message, caller_id.function_name, caller_id.file_path, caller_id.line_number) return True diff --git a/rclpy/rclpy/logging.py b/rclpy/rclpy/logging.py index dc0fa97ff..9208d01fe 100644 --- a/rclpy/rclpy/logging.py +++ b/rclpy/rclpy/logging.py @@ -15,7 +15,7 @@ from enum import IntEnum -from rclpy.impl.implementation_singleton import rclpy_logging_implementation as _rclpy_logging +from rclpy.impl.implementation_singleton import get_rclpy_logging_implementation import rclpy.impl.rcutils_logger @@ -44,11 +44,11 @@ def get_logger(name): def initialize(): - return _rclpy_logging.rclpy_logging_initialize() + return get_rclpy_logging_implementation().rclpy_logging_initialize() def shutdown(): - return _rclpy_logging.rclpy_logging_shutdown() + return get_rclpy_logging_implementation().rclpy_logging_shutdown() def clear_config(): @@ -59,9 +59,11 @@ def clear_config(): def set_logger_level(name, level): level = LoggingSeverity(level) - return _rclpy_logging.rclpy_logging_set_logger_level(name, level) + return get_rclpy_logging_implementation().rclpy_logging_set_logger_level(name, level) def get_logger_effective_level(name): - logger_level = _rclpy_logging.rclpy_logging_get_logger_effective_level(name) + logger_level = get_rclpy_logging_implementation().rclpy_logging_get_logger_effective_level( + name + ) return LoggingSeverity(logger_level) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index fa6f5601e..31d61a3bc 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -51,14 +51,13 @@ from rclpy.guard_condition import GuardCondition from rclpy.handle import Handle from rclpy.handle import InvalidHandle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.logging import get_logger from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING from rclpy.parameter_service import ParameterService from rclpy.publisher import Publisher from rclpy.qos import DeprecatedQoSProfile -from rclpy.qos import qos_profile_parameter_events -from rclpy.qos import qos_profile_services_default +from rclpy.qos import qos_profiles from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks from rclpy.qos_event import SubscriptionEventCallbacks @@ -150,7 +149,7 @@ def __init__( if not self._context.ok(): raise NotInitializedException('cannot create node') try: - self.__handle = Handle(_rclpy.rclpy_create_node( + self.__handle = Handle(get_rclpy_implementation().rclpy_create_node( node_name, namespace, self._context.handle, cli_args, use_global_arguments )) except ValueError: @@ -165,15 +164,20 @@ def __init__( # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') with self.handle as capsule: - self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) + self._logger = get_logger(get_rclpy_implementation().rclpy_get_node_logger_name( + capsule + )) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( - ParameterEvent, 'parameter_events', qos_profile_parameter_events) + ParameterEvent, 'parameter_events', qos_profiles.parameter_events) with self.handle as capsule: - self._parameter_overrides = _rclpy.rclpy_get_node_parameters(Parameter, capsule) + self._parameter_overrides = get_rclpy_implementation().rclpy_get_node_parameters( + Parameter, + capsule + ) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if parameter_overrides is not None: @@ -287,12 +291,12 @@ def handle(self, value): def get_name(self) -> str: """Get the name of the node.""" with self.handle as capsule: - return _rclpy.rclpy_get_node_name(capsule) + return get_rclpy_implementation().rclpy_get_node_name(capsule) def get_namespace(self) -> str: """Get the namespace of the node.""" with self.handle as capsule: - return _rclpy.rclpy_get_node_namespace(capsule) + return get_rclpy_implementation().rclpy_get_node_namespace(capsule) def get_clock(self) -> Clock: """Get the clock used by the node.""" @@ -1098,7 +1102,7 @@ def create_publisher( failed = False try: with self.handle as node_capsule: - publisher_capsule = _rclpy.rclpy_create_publisher( + publisher_capsule = get_rclpy_implementation().rclpy_create_publisher( node_capsule, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True @@ -1163,7 +1167,7 @@ def create_subscription( failed = False try: with self.handle as capsule: - subscription_capsule = _rclpy.rclpy_create_subscription( + subscription_capsule = get_rclpy_implementation().rclpy_create_subscription( capsule, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True @@ -1190,7 +1194,7 @@ def create_client( srv_type, srv_name: str, *, - qos_profile: QoSProfile = qos_profile_services_default, + qos_profile: Optional[QoSProfile] = None, callback_group: CallbackGroup = None ) -> Client: """ @@ -1206,9 +1210,11 @@ def create_client( callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False + if qos_profile is None: + qos_profile = qos_profiles.services_default try: with self.handle as node_capsule: - client_capsule = _rclpy.rclpy_create_client( + client_capsule = get_rclpy_implementation().rclpy_create_client( node_capsule, srv_type, srv_name, @@ -1236,7 +1242,7 @@ def create_service( srv_name: str, callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], *, - qos_profile: QoSProfile = qos_profile_services_default, + qos_profile: Optional[QoSProfile] = None, callback_group: CallbackGroup = None ) -> Service: """ @@ -1254,9 +1260,11 @@ def create_service( callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False + if qos_profile is None: + qos_profile = qos_profiles.services_default try: with self.handle as node_capsule: - service_capsule = _rclpy.rclpy_create_service( + service_capsule = get_rclpy_implementation().rclpy_create_service( node_capsule, srv_type, srv_name, @@ -1461,7 +1469,7 @@ def get_publisher_names_and_types_by_node( topic types. """ with self.handle as capsule: - return _rclpy.rclpy_get_publisher_names_and_types_by_node( + return get_rclpy_implementation().rclpy_get_publisher_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( @@ -1481,7 +1489,7 @@ def get_subscriber_names_and_types_by_node( topic types. """ with self.handle as capsule: - return _rclpy.rclpy_get_subscriber_names_and_types_by_node( + return get_rclpy_implementation().rclpy_get_subscriber_names_and_types_by_node( capsule, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( @@ -1499,7 +1507,7 @@ def get_service_names_and_types_by_node( and the second element is a list of service types. """ with self.handle as capsule: - return _rclpy.rclpy_get_service_names_and_types_by_node( + return get_rclpy_implementation().rclpy_get_service_names_and_types_by_node( capsule, node_name, node_namespace) def get_client_names_and_types_by_node( @@ -1517,7 +1525,7 @@ def get_client_names_and_types_by_node( and the second element is a list of service client types. """ with self.handle as capsule: - return _rclpy.rclpy_get_client_names_and_types_by_node( + return get_rclpy_implementation().rclpy_get_client_names_and_types_by_node( capsule, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: @@ -1530,7 +1538,10 @@ def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str topic types. """ with self.handle as capsule: - return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) + return get_rclpy_implementation().rclpy_get_topic_names_and_types( + capsule, + no_demangle + ) def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: """ @@ -1541,7 +1552,7 @@ def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: service types. """ with self.handle as capsule: - return _rclpy.rclpy_get_service_names_and_types(capsule) + return get_rclpy_implementation().rclpy_get_service_names_and_types(capsule) def get_node_names(self) -> List[str]: """ @@ -1550,7 +1561,7 @@ def get_node_names(self) -> List[str]: :return: List of node names. """ with self.handle as capsule: - names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) + names_ns = get_rclpy_implementation().rclpy_get_node_names_and_namespaces(capsule) return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: @@ -1560,7 +1571,7 @@ def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: :return: List of tuples containing two strings: the node name and node namespace. """ with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces(capsule) + return get_rclpy_implementation().rclpy_get_node_names_and_namespaces(capsule) def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) @@ -1579,7 +1590,10 @@ def count_publishers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of publishers. :return: the number of publishers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) + return self._count_publishers_or_subscribers( + topic_name, + get_rclpy_implementation().rclpy_count_publishers + ) def count_subscribers(self, topic_name: str) -> int: """ @@ -1592,7 +1606,10 @@ def count_subscribers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of subscribers. :return: the number of subscribers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) + return self._count_publishers_or_subscribers( + topic_name, + get_rclpy_implementation().rclpy_count_subscribers + ) def assert_liveliness(self) -> None: """ @@ -1602,4 +1619,4 @@ def assert_liveliness(self) -> None: application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. """ with self.handle as capsule: - _rclpy.rclpy_assert_liveliness(capsule) + get_rclpy_implementation().rclpy_assert_liveliness(capsule) diff --git a/rclpy/rclpy/parameter_service.py b/rclpy/rclpy/parameter_service.py index e3c710182..28bb69f51 100644 --- a/rclpy/rclpy/parameter_service.py +++ b/rclpy/rclpy/parameter_service.py @@ -18,7 +18,7 @@ from rcl_interfaces.srv import ListParameters, SetParameters, SetParametersAtomically from rclpy.exceptions import ParameterNotDeclaredException from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING -from rclpy.qos import qos_profile_parameters +from rclpy.qos import qos_profiles from rclpy.validate_topic_name import TOPIC_SEPARATOR_STRING @@ -32,35 +32,35 @@ def __init__(self, node): TOPIC_SEPARATOR_STRING.join((nodename, 'describe_parameters')) node.create_service( DescribeParameters, describe_parameters_service_name, - self._describe_parameters_callback, qos_profile=qos_profile_parameters + self._describe_parameters_callback, qos_profile=qos_profiles.parameters ) get_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'get_parameters')) node.create_service( GetParameters, get_parameters_service_name, self._get_parameters_callback, - qos_profile=qos_profile_parameters + qos_profile=qos_profiles.parameters ) get_parameter_types_service_name = \ TOPIC_SEPARATOR_STRING.join((nodename, 'get_parameter_types')) node.create_service( GetParameterTypes, get_parameter_types_service_name, - self._get_parameter_types_callback, qos_profile=qos_profile_parameters + self._get_parameter_types_callback, qos_profile=qos_profiles.parameters ) list_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'list_parameters')) node.create_service( ListParameters, list_parameters_service_name, self._list_parameters_callback, - qos_profile=qos_profile_parameters + qos_profile=qos_profiles.parameters ) set_parameters_service_name = TOPIC_SEPARATOR_STRING.join((nodename, 'set_parameters')) node.create_service( SetParameters, set_parameters_service_name, self._set_parameters_callback, - qos_profile=qos_profile_parameters + qos_profile=qos_profiles.parameters ) set_parameters_atomically_service_name = \ TOPIC_SEPARATOR_STRING.join((nodename, 'set_parameters_atomically')) node.create_service( SetParametersAtomically, set_parameters_atomically_service_name, self._set_parameters_atomically_callback, - qos_profile=qos_profile_parameters + qos_profile=qos_profiles.parameters ) def _describe_parameters_callback(self, request, response): diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index 818f30d98..2dc86d95e 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -16,7 +16,7 @@ from rclpy.callback_groups import CallbackGroup from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks @@ -67,7 +67,7 @@ def publish(self, msg: MsgType) -> None: if not isinstance(msg, self.msg_type): raise TypeError() with self.handle as capsule: - _rclpy.rclpy_publish(capsule, msg) + get_rclpy_implementation().rclpy_publish(capsule, msg) @property def handle(self): @@ -84,4 +84,4 @@ def assert_liveliness(self) -> None: application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. """ with self.handle as capsule: - _rclpy.rclpy_assert_liveliness(capsule) + get_rclpy_implementation().rclpy_assert_liveliness(capsule) diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index d34c76470..9a70fccaf 100644 --- a/rclpy/rclpy/qos.py +++ b/rclpy/rclpy/qos.py @@ -13,17 +13,17 @@ # limitations under the License. from argparse import Namespace -from enum import Enum from enum import IntEnum +from typing import Optional import warnings from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_action_implementation +from rclpy.impl.implementation_singleton import get_rclpy_implementation # "Forward-declare" this value so that it can be used in the QoSProfile initializer. # It will have a value by the end of definitions, before user code runs. -_qos_profile_default = None +qos_profiles = None class QoSProfile: @@ -41,11 +41,11 @@ class QoSProfile: '_avoid_ros_namespace_conventions', ] - def __init__(self, **kwargs): + def __init__(self, *, do_not_use_defaults: bool = False, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %r' % kwargs.keys() - if not _qos_profile_default: + if do_not_use_defaults or qos_profiles is None: # It is still definition time, and all calls to this initializer are expected to be # fully-defined preset profiles from the C side. assert all(kwargs[slot[1:]] is not None for slot in self.__slots__) @@ -53,7 +53,7 @@ def __init__(self, **kwargs): # if the above assertion failed. from_profile = Namespace(**{slot[1:]: None for slot in self.__slots__}) else: - from_profile = _qos_profile_default + from_profile = qos_profiles.default if 'history' not in kwargs: if 'depth' in kwargs: @@ -218,7 +218,7 @@ def avoid_ros_namespace_conventions(self, value): self._avoid_ros_namespace_conventions = value def get_c_qos_profile(self): - return _rclpy.rclpy_convert_from_py_qos_policy( + return get_rclpy_implementation().rclpy_convert_from_py_qos_policy( self.history, self.depth, self.reliability, @@ -346,44 +346,115 @@ def __init__(self, qos_profile: QoSProfile, profile_name: str): deadline=qos_profile.deadline, liveliness=qos_profile.liveliness, liveliness_lease_duration=qos_profile.liveliness_lease_duration, - avoid_ros_namespace_conventions=qos_profile.avoid_ros_namespace_conventions) + avoid_ros_namespace_conventions=qos_profile.avoid_ros_namespace_conventions, + do_not_use_defaults=True + ) self.name = profile_name -_qos_profile_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile('qos_profile_default')) -qos_profile_default = DeprecatedQoSProfile(_qos_profile_default, 'qos_profile_default') -qos_profile_system_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_system_default')) -qos_profile_sensor_data = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_sensor_data')) -qos_profile_services_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_services_default')) -qos_profile_parameters = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_parameters')) -qos_profile_parameter_events = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_parameter_events')) -qos_profile_action_status_default = QoSProfile( - **_rclpy_action.rclpy_action_get_rmw_qos_profile('rcl_action_qos_profile_status_default')) - - -class QoSPresetProfiles(Enum): - SYSTEM_DEFAULT = qos_profile_system_default - SENSOR_DATA = qos_profile_sensor_data - SERVICES_DEFAULT = qos_profile_services_default - PARAMETERS = qos_profile_parameters - PARAMETER_EVENTS = qos_profile_parameter_events - ACTION_STATUS_DEFAULT = qos_profile_action_status_default - - """Noted that the following are duplicated from QoSPolicyEnum. - - Our supported version of Python3 (3.5) doesn't have a fix that allows mixins on Enum. - """ - @classmethod - def short_keys(cls): +class _QoSProfiles: + """Implementation detail.""" + + def __init__(self): + self.__non_deprecated_default: Optional[QoSProfile] = None + self.__default: Optional[DeprecatedQoSProfile] = None + self.__system_default: Optional[DeprecatedQoSProfile] = None + self.__sensor_data: Optional[QoSProfile] = None + self.__services_default: Optional[QoSProfile] = None + self.__parameters: Optional[QoSProfile] = None + self.__parameter_events: Optional[QoSProfile] = None + self.__action_status_default: Optional[QoSProfile] = None + + def __from_rclpy_c_extension(self, name: str): + return QoSProfile( + do_not_use_defaults=True, + **get_rclpy_implementation().rclpy_get_rmw_qos_profile(name) + ) + + def __from_rclpy_action_c_extension(self, name: str): + return QoSProfile( + do_not_use_defaults=True, + **get_rclpy_action_implementation().rclpy_action_get_rmw_qos_profile(name) + ) + + @property + def _default(self): + if self.__non_deprecated_default is None: + self.__non_deprecated_default = self.__from_rclpy_c_extension('qos_profile_default') + return self.__non_deprecated_default + + @property + def default(self): + if self.__default is None: + self.__default = DeprecatedQoSProfile( + self._default, + 'qos_profile_default' + ) + return self.__default + + @property + def system_default(self): + if self.__system_default is None: + self.__system_default = self.__from_rclpy_c_extension('qos_profile_system_default') + return self.__system_default + + @property + def sensor_data(self): + if self.__sensor_data is None: + self.__sensor_data = self.__from_rclpy_c_extension('qos_profile_sensor_data') + return self.__sensor_data + + @property + def services_default(self): + if self.__services_default is None: + self.__services_default = self.__from_rclpy_c_extension('qos_profile_services_default') + return self.__services_default + + @property + def parameters(self): + if self.__parameters is None: + self.__parameters = self.__from_rclpy_c_extension('qos_profile_parameters') + return self.__parameters + + @property + def parameter_events(self): + if self.__parameter_events is None: + self.__parameter_events = self.__from_rclpy_c_extension('qos_profile_parameter_events') + return self.__parameter_events + + @property + def action_status_default(self): + if self.__action_status_default is None: + self.__action_status_default = self.__from_rclpy_action_c_extension( + 'rcl_action_qos_profile_status_default' + ) + return self.__action_status_default + + def short_keys(self): """Return a list of shortened typing-friendly enum values.""" - return [k.lower() for k in cls.__members__.keys() if not k.startswith('RMW')] + return [k for k in self.__dict__.keys() if not k.startswith('_')] - @classmethod - def get_from_short_key(cls, name): + def get_from_short_key(self, name): """Retrieve a policy type from a short name, case-insensitive.""" - return cls[name.upper()].value + return getattr(self, name.lower()) + + +""" +The following default qos profiles are available under the `qos_profiles` namespace: + - default + - system_default + - sensor_data + - services_default + - parameteres + - parameter_events + - action_status_default +They can be accessed by doing: +``` +from rclpy.qos import qos_profiles +... +def some_function_or_method(...): + ... + something_else(..., qos_profiles.default) + ... +""" +qos_profiles = _QoSProfiles() diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index e3c0ab427..ff93170f2 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -21,7 +21,7 @@ import rclpy from rclpy.callback_groups import CallbackGroup from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.waitable import NumberOfEntities from rclpy.waitable import Waitable @@ -113,7 +113,10 @@ def __init__( self._parent_handle = parent_handle with parent_handle as parent_capsule: - event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) + event_capsule = get_rclpy_implementation().rclpy_create_event( + event_type, + parent_capsule + ) self._event_handle = Handle(event_capsule) self._event_handle.requires(self._parent_handle) self._ready_to_take_data = False @@ -124,7 +127,11 @@ def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" if self._event_index is None: return False - if _rclpy.rclpy_wait_set_is_ready('event', wait_set, self._event_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'event', + wait_set, + self._event_index + ): self._ready_to_take_data = True return self._ready_to_take_data @@ -133,7 +140,11 @@ def take_data(self): if self._ready_to_take_data: self._ready_to_take_data = False with self._parent_handle as parent_capsule, self._event_handle as event_capsule: - return _rclpy.rclpy_take_event(event_capsule, parent_capsule, self.event_type) + return get_rclpy_implementation().rclpy_take_event( + event_capsule, + parent_capsule, + self.event_type + ) return None async def execute(self, taken_data): @@ -149,7 +160,11 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entites to wait set.""" with self._event_handle as event_capsule: - self._event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, event_capsule) + self._event_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'event', + wait_set, + event_capsule + ) # End Waitable API diff --git a/rclpy/rclpy/service.py b/rclpy/rclpy/service.py index 0084e40b5..fe927ebff 100644 --- a/rclpy/rclpy/service.py +++ b/rclpy/rclpy/service.py @@ -1,4 +1,4 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. +# Copyright 2016-2019 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. @@ -16,7 +16,7 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.qos import QoSProfile # Used for documentation purposes only @@ -71,7 +71,7 @@ def send_response(self, response: SrvTypeResponse, header) -> None: if not isinstance(response, self.srv_type.Response): raise TypeError() with self.handle as capsule: - _rclpy.rclpy_send_response(capsule, response, header) + get_rclpy_implementation().rclpy_send_response(capsule, response, header) @property def handle(self): diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py index 0dc425af7..6cbc156b7 100644 --- a/rclpy/rclpy/signals.py +++ b/rclpy/rclpy/signals.py @@ -14,7 +14,7 @@ from rclpy.guard_condition import GuardCondition from rclpy.handle import InvalidHandle -from rclpy.impl.implementation_singleton import rclpy_signal_handler_implementation as _signals +from rclpy.impl.implementation_singleton import get_rclpy_signal_handler_implementation class SignalHandlerGuardCondition(GuardCondition): @@ -22,7 +22,9 @@ class SignalHandlerGuardCondition(GuardCondition): def __init__(self, context=None): super().__init__(callback=None, callback_group=None, context=context) with self.handle as capsule: - _signals.rclpy_register_sigint_guard_condition(capsule) + get_rclpy_signal_handler_implementation().rclpy_register_sigint_guard_condition( + capsule + ) def __del__(self): try: @@ -36,5 +38,7 @@ def __del__(self): def destroy(self): with self.handle as capsule: - _signals.rclpy_unregister_sigint_guard_condition(capsule) + get_rclpy_signal_handler_implementation().rclpy_unregister_sigint_guard_condition( + capsule + ) super().destroy() diff --git a/rclpy/rclpy/time.py b/rclpy/rclpy/time.py index 397cfb7ed..1dd90e4c3 100644 --- a/rclpy/rclpy/time.py +++ b/rclpy/rclpy/time.py @@ -1,4 +1,4 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. +# Copyright 2018-2019 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. @@ -15,7 +15,7 @@ import builtin_interfaces from rclpy.clock import ClockType from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation CONVERSION_CONSTANT = 10 ** 9 @@ -33,7 +33,10 @@ def __init__(self, *, seconds=0, nanoseconds=0, clock_type=ClockType.SYSTEM_TIME total_nanoseconds = int(seconds * CONVERSION_CONSTANT) total_nanoseconds += int(nanoseconds) try: - self._time_handle = _rclpy.rclpy_create_time_point(total_nanoseconds, clock_type) + self._time_handle = get_rclpy_implementation().rclpy_create_time_point( + total_nanoseconds, + clock_type + ) except OverflowError as e: raise OverflowError( 'Total nanoseconds value is too large to store in C time point.') from e @@ -41,7 +44,7 @@ def __init__(self, *, seconds=0, nanoseconds=0, clock_type=ClockType.SYSTEM_TIME @property def nanoseconds(self): - return _rclpy.rclpy_time_point_get_nanoseconds(self._time_handle) + return get_rclpy_implementation().rclpy_time_point_get_nanoseconds(self._time_handle) def seconds_nanoseconds(self): """ diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index c96ea3798..ec1aac5f1 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -15,7 +15,7 @@ from rclpy.clock import Clock from rclpy.clock import ClockType from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.utilities import get_default_context @@ -27,7 +27,7 @@ def __init__(self, callback, callback_group, timer_period_ns, *, context=None): # TODO(sloretz) Allow passing clocks in via timer constructor self._clock = Clock(clock_type=ClockType.STEADY_TIME) with self._clock.handle as clock_capsule: - self.__handle = Handle(_rclpy.rclpy_create_timer( + self.__handle = Handle(get_rclpy_implementation().rclpy_create_timer( clock_capsule, self._context.handle, timer_period_ns)) self.__handle.requires(self._clock.handle) self.timer_period_ns = timer_period_ns @@ -50,7 +50,7 @@ def clock(self): @property def timer_period_ns(self): with self.handle as capsule: - val = _rclpy.rclpy_get_timer_period(capsule) + val = get_rclpy_implementation().rclpy_get_timer_period(capsule) self._timer_period_ns = val return val @@ -58,29 +58,29 @@ def timer_period_ns(self): def timer_period_ns(self, value): val = int(value) with self.handle as capsule: - _rclpy.rclpy_change_timer_period(capsule, val) + get_rclpy_implementation().rclpy_change_timer_period(capsule, val) self._timer_period_ns = val def is_ready(self): with self.handle as capsule: - return _rclpy.rclpy_is_timer_ready(capsule) + return get_rclpy_implementation().rclpy_is_timer_ready(capsule) def is_canceled(self): with self.handle as capsule: - return _rclpy.rclpy_is_timer_canceled(capsule) + return get_rclpy_implementation().rclpy_is_timer_canceled(capsule) def cancel(self): with self.handle as capsule: - _rclpy.rclpy_cancel_timer(capsule) + get_rclpy_implementation().rclpy_cancel_timer(capsule) def reset(self): with self.handle as capsule: - _rclpy.rclpy_reset_timer(capsule) + get_rclpy_implementation().rclpy_reset_timer(capsule) def time_since_last_call(self): with self.handle as capsule: - return _rclpy.rclpy_time_since_last_call(capsule) + return get_rclpy_implementation().rclpy_time_since_last_call(capsule) def time_until_next_call(self): with self.handle as capsule: - return _rclpy.rclpy_time_until_next_call(capsule) + return get_rclpy_implementation().rclpy_time_until_next_call(capsule) diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py index bba7ed731..020962f41 100644 --- a/rclpy/rclpy/utilities.py +++ b/rclpy/rclpy/utilities.py @@ -17,6 +17,7 @@ from rclpy.constants import S_TO_NS from rclpy.context import Context +from rclpy.impl.implementation_singleton import get_rclpy_implementation g_default_context = None g_context_lock = threading.Lock() @@ -37,9 +38,7 @@ def get_default_context(*, shutting_down=False): def remove_ros_args(args=None): - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - return rclpy_implementation.rclpy_remove_ros_args( + return get_rclpy_implementation().rclpy_remove_ros_args( args if args is not None else sys.argv) @@ -63,9 +62,7 @@ def try_shutdown(*, context=None): def get_rmw_implementation_identifier(): - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - return rclpy_implementation.rclpy_get_rmw_implementation_identifier() + return get_rclpy_implementation().rclpy_get_rmw_implementation_identifier() def timeout_sec_to_nsec(timeout_sec): diff --git a/rclpy/rclpy/validate_full_topic_name.py b/rclpy/rclpy/validate_full_topic_name.py index afbf34b54..d054557d7 100644 --- a/rclpy/rclpy/validate_full_topic_name.py +++ b/rclpy/rclpy/validate_full_topic_name.py @@ -14,7 +14,7 @@ from rclpy.exceptions import InvalidServiceNameException from rclpy.exceptions import InvalidTopicNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation def validate_full_topic_name(name, *, is_service=False): @@ -31,7 +31,7 @@ def validate_full_topic_name(name, *, is_service=False): :returns: True when it is valid :raises: InvalidTopicNameException: when the name is invalid """ - result = _rclpy.rclpy_get_validation_error_for_full_topic_name(name) + result = get_rclpy_implementation().rclpy_get_validation_error_for_full_topic_name(name) if result is None: return True error_msg, invalid_index = result diff --git a/rclpy/rclpy/validate_namespace.py b/rclpy/rclpy/validate_namespace.py index 3e1dcf66f..e3a3f280b 100644 --- a/rclpy/rclpy/validate_namespace.py +++ b/rclpy/rclpy/validate_namespace.py @@ -13,7 +13,7 @@ # limitations under the License. from rclpy.exceptions import InvalidNamespaceException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation def validate_namespace(namespace): @@ -31,7 +31,9 @@ def validate_namespace(namespace): :returns: True when it is valid :raises: InvalidNamespaceException: when the namespace is invalid """ - result = _rclpy.rclpy_get_validation_error_for_namespace(namespace) + result = get_rclpy_implementation().rclpy_get_validation_error_for_namespace( + namespace + ) if result is None: return True error_msg, invalid_index = result diff --git a/rclpy/rclpy/validate_node_name.py b/rclpy/rclpy/validate_node_name.py index de14be4b7..32c89ffc2 100644 --- a/rclpy/rclpy/validate_node_name.py +++ b/rclpy/rclpy/validate_node_name.py @@ -13,7 +13,7 @@ # limitations under the License. from rclpy.exceptions import InvalidNodeNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation def validate_node_name(node_name): @@ -27,7 +27,7 @@ def validate_node_name(node_name): :returns: True when it is valid :raises: InvalidNodeNameException: when the node_name is invalid """ - result = _rclpy.rclpy_get_validation_error_for_node_name(node_name) + result = get_rclpy_implementation().rclpy_get_validation_error_for_node_name(node_name) if result is None: return True error_msg, invalid_index = result diff --git a/rclpy/rclpy/validate_topic_name.py b/rclpy/rclpy/validate_topic_name.py index 08712e4c0..1da87faa2 100644 --- a/rclpy/rclpy/validate_topic_name.py +++ b/rclpy/rclpy/validate_topic_name.py @@ -14,7 +14,7 @@ from rclpy.exceptions import InvalidServiceNameException from rclpy.exceptions import InvalidTopicNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation TOPIC_SEPARATOR_STRING = '/' @@ -33,7 +33,7 @@ def validate_topic_name(name, *, is_service=False): :returns: True when it is valid :raises: InvalidTopicNameException: when the name is invalid """ - result = _rclpy.rclpy_get_validation_error_for_topic_name(name) + result = get_rclpy_implementation().rclpy_get_validation_error_for_topic_name(name) if result is None: return True error_msg, invalid_index = result diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index b93dafab5..fde61eab4 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -33,9 +33,9 @@ from rclpy.exceptions import ParameterImmutableException from rclpy.exceptions import ParameterNotDeclaredException from rclpy.executors import SingleThreadedExecutor +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.parameter import Parameter -from rclpy.qos import qos_profile_default -from rclpy.qos import qos_profile_sensor_data +from rclpy.qos import qos_profiles from rclpy.time_source import USE_SIM_TIME_NAME from test_msgs.msg import BasicTypes @@ -69,7 +69,7 @@ def test_accessors(self): def test_create_publisher(self): self.node.create_publisher(BasicTypes, 'chatter', 0) self.node.create_publisher(BasicTypes, 'chatter', 1) - self.node.create_publisher(BasicTypes, 'chatter', qos_profile_sensor_data) + self.node.create_publisher(BasicTypes, 'chatter', qos_profiles.sensor_data) with self.assertRaisesRegex(InvalidTopicNameException, 'must not contain characters'): self.node.create_publisher(BasicTypes, 'chatter?', 1) with self.assertRaisesRegex(InvalidTopicNameException, 'must not start with a number'): @@ -85,7 +85,7 @@ def test_create_subscription(self): self.node.create_subscription(BasicTypes, 'chatter', lambda msg: print(msg), 0) self.node.create_subscription(BasicTypes, 'chatter', lambda msg: print(msg), 1) self.node.create_subscription( - BasicTypes, 'chatter', lambda msg: print(msg), qos_profile_sensor_data) + BasicTypes, 'chatter', lambda msg: print(msg), qos_profiles.sensor_data) with self.assertRaisesRegex(InvalidTopicNameException, 'must not contain characters'): self.node.create_subscription(BasicTypes, 'chatter?', lambda msg: print(msg), 1) with self.assertRaisesRegex(InvalidTopicNameException, 'must not start with a number'): @@ -153,7 +153,7 @@ def test_deprecation_warnings(self): assert issubclass(w[0].category, UserWarning) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') - self.node.create_publisher(BasicTypes, 'chatter', qos_profile_default) + self.node.create_publisher(BasicTypes, 'chatter', qos_profiles.default) assert len(w) == 1 assert issubclass(w[0].category, UserWarning) with warnings.catch_warnings(record=True) as w: @@ -164,7 +164,7 @@ def test_deprecation_warnings(self): with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always') self.node.create_subscription( - BasicTypes, 'chatter', lambda msg: print(msg), qos_profile_default) + BasicTypes, 'chatter', lambda msg: print(msg), qos_profiles.default) assert len(w) == 1 assert issubclass(w[0].category, UserWarning) with warnings.catch_warnings(record=True) as w: @@ -1564,10 +1564,11 @@ def test_bad_node_arguments(self): context = rclpy.context.Context() rclpy.init(context=context) - from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - invalid_ros_args_error_pattern = r'Failed to parse ROS arguments:.*not-a-remap.*' - with self.assertRaisesRegex(_rclpy.RCLInvalidROSArgsError, invalid_ros_args_error_pattern): + with self.assertRaisesRegex( + get_rclpy_implementation().RCLInvalidROSArgsError, + invalid_ros_args_error_pattern + ): rclpy.create_node( 'my_node', namespace='/my_ns', @@ -1575,7 +1576,10 @@ def test_bad_node_arguments(self): context=context) unknown_ros_args_error_pattern = r'Found unknown ROS arguments:.*\[\'--my-custom-flag\'\]' - with self.assertRaisesRegex(_rclpy.UnknownROSArgsError, unknown_ros_args_error_pattern): + with self.assertRaisesRegex( + get_rclpy_implementation().UnknownROSArgsError, + unknown_ros_args_error_pattern + ): rclpy.create_node( 'my_node', namespace='/my_ns', diff --git a/rclpy/test/test_qos.py b/rclpy/test/test_qos.py index d4f8caa01..d45ff7fb7 100644 --- a/rclpy/test/test_qos.py +++ b/rclpy/test/test_qos.py @@ -16,13 +16,11 @@ import warnings from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import _qos_profile_default -from rclpy.qos import qos_profile_system_default +from rclpy.impl.implementation_singleton import get_rclpy_implementation +from rclpy.qos import qos_profiles from rclpy.qos import QoSDurabilityPolicy from rclpy.qos import QoSHistoryPolicy from rclpy.qos import QoSLivelinessPolicy -from rclpy.qos import QoSPresetProfiles from rclpy.qos import QoSProfile from rclpy.qos import QoSReliabilityPolicy @@ -31,7 +29,9 @@ class TestQosProfile(unittest.TestCase): def convert_and_assert_equality(self, qos_profile): c_profile = qos_profile.get_c_qos_profile() - converted_profile = QoSProfile(**_rclpy.rclpy_convert_to_py_qos_policy(c_profile)) + converted_profile = QoSProfile(**get_rclpy_implementation().rclpy_convert_to_py_qos_policy( + c_profile + )) self.assertEqual(qos_profile, converted_profile) def test_depth_only_constructor(self): @@ -139,15 +139,14 @@ def test_policy_short_names(self): def test_preset_profiles(self): # Make sure the Enum does what we expect - assert QoSPresetProfiles.SYSTEM_DEFAULT.value == qos_profile_system_default assert ( - QoSPresetProfiles.SYSTEM_DEFAULT.value == - QoSPresetProfiles.get_from_short_key('system_default')) + qos_profiles.system_default == + qos_profiles.get_from_short_key('system_default')) def test_default_profile(self): with warnings.catch_warnings(record=True): warnings.simplefilter('always') profile = QoSProfile() assert all( - profile.__getattribute__(k) == _qos_profile_default.__getattribute__(k) + profile.__getattribute__(k) == qos_profiles._default.__getattribute__(k) for k in profile.__slots__) diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 88ad7c1be..a27025e56 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -17,7 +17,7 @@ import rclpy from rclpy.handle import Handle -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.qos_event import PublisherEventCallbacks from rclpy.qos_event import QoSLivelinessChangedInfo from rclpy.qos_event import QoSLivelinessLostInfo @@ -106,7 +106,10 @@ def test_subscription_constructor(self): def _create_event_handle(self, parent_entity, event_type): with parent_entity.handle as parent_capsule: - event_capsule = _rclpy.rclpy_create_event(event_type, parent_capsule) + event_capsule = get_rclpy_implementation().rclpy_create_event( + event_type, + parent_capsule + ) self.assertIsNotNone(event_capsule) return Handle(event_capsule) @@ -135,32 +138,49 @@ def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, 'test_topic', 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) + wait_set = get_rclpy_implementation().rclpy_get_zero_initialized_wait_set() + get_rclpy_implementation().rclpy_wait_set_init( + wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) with deadline_event_handle as capsule: - deadline_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + deadline_event_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'event', + wait_set, + capsule + ) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) with liveliness_event_handle as capsule: - liveliness_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + liveliness_event_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'event', + wait_set, + capsule + ) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + get_rclpy_implementation().rclpy_wait(wait_set, 0) + self.assertFalse(get_rclpy_implementation().rclpy_wait_set_is_ready( + 'event', + wait_set, + deadline_event_index + )) + self.assertFalse(get_rclpy_implementation().rclpy_wait_set_is_ready( + 'event', + wait_set, + liveliness_event_index + )) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side try: with deadline_event_handle as event_capsule, publisher.handle as publisher_capsule: - event_data = _rclpy.rclpy_take_event( + event_data = get_rclpy_implementation().rclpy_take_event( event_capsule, publisher_capsule, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) @@ -172,7 +192,7 @@ def test_call_publisher_rclpy_event_apis(self): try: with liveliness_event_handle as event_capsule, publisher.handle as publisher_capsule: - event_data = _rclpy.rclpy_take_event( + event_data = get_rclpy_implementation().rclpy_take_event( event_capsule, publisher_capsule, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) @@ -188,32 +208,49 @@ def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, 'test_topic', Mock(), 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) + wait_set = get_rclpy_implementation().rclpy_get_zero_initialized_wait_set() + get_rclpy_implementation().rclpy_wait_set_init( + wait_set, 0, 0, 0, 0, 0, 2, self.context.handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) with deadline_event_handle as capsule: - deadline_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + deadline_event_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'event', + wait_set, + capsule + ) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) with liveliness_event_handle as capsule: - liveliness_event_index = _rclpy.rclpy_wait_set_add_entity('event', wait_set, capsule) + liveliness_event_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'event', + wait_set, + capsule + ) self.assertIsNotNone(liveliness_event_index) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + get_rclpy_implementation().rclpy_wait(wait_set, 0) + self.assertFalse(get_rclpy_implementation().rclpy_wait_set_is_ready( + 'event', + wait_set, + deadline_event_index + )) + self.assertFalse(get_rclpy_implementation().rclpy_wait_set_is_ready( + 'event', + wait_set, + liveliness_event_index + )) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side try: with deadline_event_handle as event_capsule, subscription.handle as parent_capsule: - event_data = _rclpy.rclpy_take_event( + event_data = get_rclpy_implementation().rclpy_take_event( event_capsule, parent_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) @@ -225,7 +262,7 @@ def test_call_subscription_rclpy_event_apis(self): try: with liveliness_event_handle as event_capsule, subscription.handle as parent_capsule: - event_data = _rclpy.rclpy_take_event( + event_data = get_rclpy_implementation().rclpy_take_event( event_capsule, parent_capsule, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 0afbc8f05..649aebc45 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -21,7 +21,7 @@ from rclpy.clock import Clock from rclpy.clock import ClockType from rclpy.executors import SingleThreadedExecutor -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from rclpy.impl.implementation_singleton import get_rclpy_implementation from rclpy.node import check_for_type_support from rclpy.qos import QoSProfile from rclpy.task import Future @@ -42,7 +42,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle as node_capsule: - self.client = _rclpy.rclpy_create_client( + self.client = get_rclpy_implementation().rclpy_create_client( node_capsule, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -52,7 +52,11 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('client', wait_set, self.client_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'client', + wait_set, + self.client_index + ): self.client_is_ready = True return self.client_is_ready @@ -60,7 +64,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.client_is_ready: self.client_is_ready = False - return _rclpy.rclpy_take_response(self.client, EmptySrv.Response) + return get_rclpy_implementation().rclpy_take_response(self.client, EmptySrv.Response) return None async def execute(self, taken_data): @@ -76,7 +80,11 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.client_index = _rclpy.rclpy_wait_set_add_entity('client', wait_set, self.client) + self.client_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'client', + wait_set, + self.client + ) class ServerWaitable(Waitable): @@ -85,7 +93,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle as node_capsule: - self.server = _rclpy.rclpy_create_service( + self.server = get_rclpy_implementation().rclpy_create_service( node_capsule, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) self.server_index = None self.server_is_ready = False @@ -95,7 +103,11 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('service', wait_set, self.server_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'service', + wait_set, + self.server_index + ): self.server_is_ready = True return self.server_is_ready @@ -103,7 +115,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.server_is_ready: self.server_is_ready = False - return _rclpy.rclpy_take_request(self.server, EmptySrv.Request) + return get_rclpy_implementation().rclpy_take_request(self.server, EmptySrv.Request) return None async def execute(self, taken_data): @@ -119,7 +131,11 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.server_index = _rclpy.rclpy_wait_set_add_entity('service', wait_set, self.server) + self.server_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'service', + wait_set, + self.server + ) class TimerWaitable(Waitable): @@ -130,7 +146,7 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 with self._clock.handle as clock_capsule: - self.timer = _rclpy.rclpy_create_timer( + self.timer = get_rclpy_implementation().rclpy_create_timer( clock_capsule, node.context.handle, period_nanoseconds) self.timer_index = None self.timer_is_ready = False @@ -140,7 +156,11 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('timer', wait_set, self.timer_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'timer', + wait_set, + self.timer_index + ): self.timer_is_ready = True return self.timer_is_ready @@ -148,7 +168,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.timer_is_ready: self.timer_is_ready = False - _rclpy.rclpy_call_timer(self.timer) + get_rclpy_implementation().rclpy_call_timer(self.timer) return 'timer' return None @@ -165,7 +185,11 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.timer_index = _rclpy.rclpy_wait_set_add_entity('timer', wait_set, self.timer) + self.timer_index = get_rclpy_implementation().rclpy_wait_set_add_entity( + 'timer', + wait_set, + self.timer + ) class SubscriptionWaitable(Waitable): @@ -174,7 +198,7 @@ def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle as node_capsule: - self.subscription = _rclpy.rclpy_create_subscription( + self.subscription = get_rclpy_implementation().rclpy_create_subscription( node_capsule, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) self.subscription_index = None self.subscription_is_ready = False @@ -184,7 +208,11 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('subscription', wait_set, self.subscription_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'subscription', + wait_set, + self.subscription_index + ): self.subscription_is_ready = True return self.subscription_is_ready @@ -192,7 +220,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.subscription_is_ready: self.subscription_is_ready = False - return _rclpy.rclpy_take(self.subscription, EmptyMsg, False) + return get_rclpy_implementation().rclpy_take(self.subscription, EmptyMsg, False) return None async def execute(self, taken_data): @@ -208,7 +236,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.subscription_index = _rclpy.rclpy_wait_set_add_entity( + self.subscription_index = get_rclpy_implementation().rclpy_wait_set_add_entity( 'subscription', wait_set, self.subscription) @@ -217,7 +245,9 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - self.guard_condition = _rclpy.rclpy_create_guard_condition(node.context.handle) + self.guard_condition = get_rclpy_implementation().rclpy_create_guard_condition( + node.context.handle + ) self.guard_condition_index = None self.guard_is_ready = False @@ -226,7 +256,11 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('guard_condition', wait_set, self.guard_condition_index): + if get_rclpy_implementation().rclpy_wait_set_is_ready( + 'guard_condition', + wait_set, + self.guard_condition_index + ): self.guard_is_ready = True return self.guard_is_ready @@ -250,7 +284,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( + self.guard_condition_index = get_rclpy_implementation().rclpy_wait_set_add_entity( 'guard_condition', wait_set, self.guard_condition) @@ -314,11 +348,13 @@ def test_waitable_with_client(self): server = self.node.create_service(EmptySrv, 'test_client', lambda req, resp: resp) - while not _rclpy.rclpy_service_server_is_available(self.waitable.client): + while not get_rclpy_implementation().rclpy_service_server_is_available( + self.waitable.client + ): time.sleep(0.1) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_send_request(self.waitable.client, EmptySrv.Request()) + get_rclpy_implementation().rclpy_send_request(self.waitable.client, EmptySrv.Request()) thr.join() assert self.waitable.future.done() @@ -366,7 +402,9 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) + get_rclpy_implementation().rclpy_trigger_guard_condition( + self.waitable.guard_condition + ) thr.join() assert self.waitable.future.done()