From 4b10d2e988a72a150db4564ca272beada0ea4c17 Mon Sep 17 00:00:00 2001 From: offish Date: Thu, 31 Oct 2024 10:42:39 +0100 Subject: [PATCH] test submodule --- .gitmodules | 3 + rclpy | 1 + src/rclpy | 1 + src/rclpy/__init__.py | 253 --- src/rclpy/action/__init__.py | 19 - src/rclpy/action/client.py | 596 ------ src/rclpy/action/graph.py | 71 - src/rclpy/action/server.py | 631 ------- src/rclpy/callback_groups.py | 117 -- src/rclpy/client.py | 187 -- src/rclpy/clock.py | 284 --- src/rclpy/constants.py | 15 - src/rclpy/context.py | 151 -- src/rclpy/duration.py | 108 -- src/rclpy/exceptions.py | 149 -- src/rclpy/executors.py | 801 -------- src/rclpy/expand_topic_name.py | 33 - src/rclpy/guard_condition.py | 41 - src/rclpy/impl/__init__.py | 13 - src/rclpy/impl/implementation_singleton.py | 32 - src/rclpy/impl/logging_severity.py | 33 - src/rclpy/impl/rcutils_logger.py | 349 ---- src/rclpy/lifecycle/__init__.py | 44 - src/rclpy/lifecycle/managed_entity.py | 81 - src/rclpy/lifecycle/node.py | 439 ----- src/rclpy/lifecycle/publisher.py | 37 - src/rclpy/logging.py | 68 - src/rclpy/node.py | 1951 -------------------- src/rclpy/parameter.py | 213 --- src/rclpy/parameter_service.py | 172 -- src/rclpy/publisher.py | 123 -- src/rclpy/qos.py | 499 ----- src/rclpy/qos_event.py | 291 --- src/rclpy/qos_overriding_options.py | 190 -- src/rclpy/serialization.py | 42 - src/rclpy/service.py | 87 - src/rclpy/signals.py | 67 - src/rclpy/subscription.py | 85 - src/rclpy/task.py | 273 --- src/rclpy/time.py | 147 -- src/rclpy/time_source.py | 143 -- src/rclpy/timer.py | 145 -- src/rclpy/topic_endpoint_info.py | 180 -- src/rclpy/topic_or_service_is_hidden.py | 31 - src/rclpy/type_support.py | 61 - src/rclpy/utilities.py | 135 -- src/rclpy/validate_full_topic_name.py | 41 - src/rclpy/validate_namespace.py | 38 - src/rclpy/validate_node_name.py | 34 - src/rclpy/validate_parameter_name.py | 34 - src/rclpy/validate_topic_name.py | 43 - src/rclpy/wait_for_message.py | 71 - src/rclpy/waitable.py | 100 - 53 files changed, 5 insertions(+), 9748 deletions(-) create mode 100644 .gitmodules create mode 160000 rclpy create mode 120000 src/rclpy delete mode 100644 src/rclpy/__init__.py delete mode 100644 src/rclpy/action/__init__.py delete mode 100644 src/rclpy/action/client.py delete mode 100644 src/rclpy/action/graph.py delete mode 100644 src/rclpy/action/server.py delete mode 100644 src/rclpy/callback_groups.py delete mode 100644 src/rclpy/client.py delete mode 100644 src/rclpy/clock.py delete mode 100644 src/rclpy/constants.py delete mode 100644 src/rclpy/context.py delete mode 100644 src/rclpy/duration.py delete mode 100644 src/rclpy/exceptions.py delete mode 100644 src/rclpy/executors.py delete mode 100644 src/rclpy/expand_topic_name.py delete mode 100644 src/rclpy/guard_condition.py delete mode 100644 src/rclpy/impl/__init__.py delete mode 100644 src/rclpy/impl/implementation_singleton.py delete mode 100644 src/rclpy/impl/logging_severity.py delete mode 100644 src/rclpy/impl/rcutils_logger.py delete mode 100644 src/rclpy/lifecycle/__init__.py delete mode 100644 src/rclpy/lifecycle/managed_entity.py delete mode 100644 src/rclpy/lifecycle/node.py delete mode 100644 src/rclpy/lifecycle/publisher.py delete mode 100644 src/rclpy/logging.py delete mode 100644 src/rclpy/node.py delete mode 100644 src/rclpy/parameter.py delete mode 100644 src/rclpy/parameter_service.py delete mode 100644 src/rclpy/publisher.py delete mode 100644 src/rclpy/qos.py delete mode 100644 src/rclpy/qos_event.py delete mode 100644 src/rclpy/qos_overriding_options.py delete mode 100644 src/rclpy/serialization.py delete mode 100644 src/rclpy/service.py delete mode 100644 src/rclpy/signals.py delete mode 100644 src/rclpy/subscription.py delete mode 100644 src/rclpy/task.py delete mode 100644 src/rclpy/time.py delete mode 100644 src/rclpy/time_source.py delete mode 100644 src/rclpy/timer.py delete mode 100644 src/rclpy/topic_endpoint_info.py delete mode 100644 src/rclpy/topic_or_service_is_hidden.py delete mode 100644 src/rclpy/type_support.py delete mode 100644 src/rclpy/utilities.py delete mode 100644 src/rclpy/validate_full_topic_name.py delete mode 100644 src/rclpy/validate_namespace.py delete mode 100644 src/rclpy/validate_node_name.py delete mode 100644 src/rclpy/validate_parameter_name.py delete mode 100644 src/rclpy/validate_topic_name.py delete mode 100644 src/rclpy/wait_for_message.py delete mode 100644 src/rclpy/waitable.py diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..c84f16c --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "rclpy"] + path = rclpy + url = https://github.com/ros2/rclpy.git diff --git a/rclpy b/rclpy new file mode 160000 index 0000000..d387309 --- /dev/null +++ b/rclpy @@ -0,0 +1 @@ +Subproject commit d387309e304fbdf73e8b54e6f1f1c6bfaa593545 diff --git a/src/rclpy b/src/rclpy new file mode 120000 index 0000000..7cc1413 --- /dev/null +++ b/src/rclpy @@ -0,0 +1 @@ +../rclpy/rclpy/rclpy/ \ No newline at end of file diff --git a/src/rclpy/__init__.py b/src/rclpy/__init__.py deleted file mode 100644 index 1548c08..0000000 --- a/src/rclpy/__init__.py +++ /dev/null @@ -1,253 +0,0 @@ -# Copyright 2016-2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -A collection of functions for writing a ROS program. - -A typical ROS program consists of the following operations: - -#. Initialization -#. Create one or more ROS nodes -#. Process node callbacks -#. Shutdown - -Inititalization is done by calling :func:`init` for a particular :class:`.Context`. -This must be done before any ROS nodes can be created. - -Creating a ROS node is done by calling :func:`create_node` or by instantiating a -:class:`.Node`. -A node can be used to create common ROS entities like publishers, subscriptions, services, -and actions. - -After a node is created, items of work can be done (e.g. subscription callbacks) by *spinning* on -the node. -The following functions can be used to process work that is waiting to be executed: :func:`spin`, -:func:`spin_once`, and :func:`spin_until_future_complete`. - -When finished with a previously initialized :class:`.Context` (ie. done using -all ROS nodes associated with the context), the :func:`shutdown` function should be called. -This will invalidate all entities derived from the context. -""" - -from typing import List -from typing import Optional -from typing import TYPE_CHECKING - -from rclpy.context import Context -from rclpy.parameter import Parameter -from rclpy.signals import install_signal_handlers -from rclpy.signals import SignalHandlerOptions -from rclpy.signals import uninstall_signal_handlers -from rclpy.task import Future -from rclpy.utilities import get_default_context -from rclpy.utilities import get_rmw_implementation_identifier # noqa: F401 -from rclpy.utilities import ok # noqa: F401 forwarding to this module -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 - - -__version__ = '1.0.0' - - -def init( - *, - args: Optional[List[str]] = None, - context: Context = None, - domain_id: Optional[int] = None, - signal_handler_options: Optional[SignalHandlerOptions] = None, -) -> None: - """ - Initialize ROS communications for a given context. - - :param args: List of command line arguments. - :param context: The context to initialize. If ``None``, then the default context is used - (see :func:`.get_default_context`). - :param domain_id: ROS domain id. - :param signal_handler_options: Indicate which signal handlers to install. - If `None`, SIGINT and SIGTERM will be installed when initializing the default context. - """ - context = get_default_context() if context is None else context - if signal_handler_options is None: - if context is None or context is get_default_context(): - signal_handler_options = SignalHandlerOptions.ALL - else: - signal_handler_options = SignalHandlerOptions.NO - install_signal_handlers(signal_handler_options) - return context.init(args, domain_id=domain_id) - - -# The global spin functions need an executor to do the work -# A persistent executor can re-run async tasks that yielded in rclpy.spin*(). -__executor = None - - -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() - context = get_default_context() - - def reset_executor(): - global __executor - __executor.shutdown() - __executor = None - context.on_shutdown(reset_executor) - return __executor - - -def shutdown(*, context: Context = None, uninstall_handlers: Optional[bool] = None) -> None: - """ - Shutdown a previously initialized context. - - This will also shutdown the global executor. - - :param context: The context to invalidate. If ``None``, then the default context is used - (see :func:`.get_default_context`). - :param uninstall_handlers: - If `None`, signal handlers will be uninstalled when shutting down the default context. - If `True`, signal handlers will be uninstalled. - If not, signal handlers won't be uninstalled. - """ - _shutdown(context=context) - if ( - uninstall_handlers or ( - uninstall_handlers is None and ( - context is None or context is get_default_context())) - ): - uninstall_signal_handlers() - - -def create_node( - node_name: str, - *, - context: Context = None, - cli_args: List[str] = None, - namespace: str = None, - use_global_arguments: bool = True, - enable_rosout: bool = True, - start_parameter_services: bool = True, - parameter_overrides: List[Parameter] = None, - allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False -) -> 'Node': - """ - Create an instance of :class:`.Node`. - - :param node_name: A name to give to the node. - :param context: The context to associated with the node, or ``None`` for the default global - context. - :param cli_args: Command line arguments to be used by the node. Being specific to a ROS node, - an implicit `--ros-args` scope flag always precedes these arguments. - :param namespace: The namespace prefix to apply to entities associated with the node - (node name, topics, etc). - :param use_global_arguments: ``False`` if the node should ignore process-wide command line - arguments. - :param enable_rosout: ``False`` if the node should ignore rosout logging. - :param start_parameter_services: ``False`` if the node should not create parameter services. - :param parameter_overrides: A list of :class:`.Parameter` which are used to override the - initial values of parameters declared on this node. - :param allow_undeclared_parameters: if True undeclared parameters are allowed, default False. - This option doesn't affect `parameter_overrides`. - :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" will - 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, - enable_rosout=enable_rosout, - start_parameter_services=start_parameter_services, - parameter_overrides=parameter_overrides, - allow_undeclared_parameters=allow_undeclared_parameters, - automatically_declare_parameters_from_overrides=( - automatically_declare_parameters_from_overrides - )) - - -def spin_once(node: 'Node', *, executor: 'Executor' = None, timeout_sec: float = None) -> None: - """ - Execute one item of work or wait until a timeout expires. - - One callback will be executed by the provided executor as long as that callback is ready - before the timeout expires. - - If no executor is provided (ie. ``None``), then the global executor is used. - It is possible the work done is for a node other than the one provided if the global executor - has a partially completed coroutine. - - :param node: A node to add to the executor to check for work. - :param executor: The executor to use, or the global executor if ``None``. - :param timeout_sec: Seconds to wait. Block forever if ``None`` or negative. Don't wait if 0. - """ - executor = get_global_executor() if executor is None else executor - try: - executor.add_node(node) - executor.spin_once(timeout_sec=timeout_sec) - finally: - executor.remove_node(node) - - -def spin(node: 'Node', executor: 'Executor' = None) -> None: - """ - Execute work and block until the context associated with the executor is shutdown. - - Callbacks will be executed by the provided executor. - - This function blocks. - - :param node: A node to add to the executor to check for work. - :param executor: The executor to use, or the global executor if ``None``. - """ - executor = get_global_executor() if executor is None else executor - try: - executor.add_node(node) - while executor.context.ok(): - executor.spin_once() - finally: - executor.remove_node(node) - - -def spin_until_future_complete( - node: 'Node', - future: Future, - executor: 'Executor' = None, - timeout_sec: float = None -) -> None: - """ - Execute work until the future is complete. - - Callbacks and other work will be executed by the provided executor until ``future.done()`` - returns ``True`` or the context associated with the executor is shutdown. - - :param node: A node to add to the executor to check for work. - :param future: The future object to wait on. - :param executor: The executor to use, or the global executor if ``None``. - :param timeout_sec: Seconds to wait. Block until the future is complete - if ``None`` or negative. Don't wait if 0. - """ - executor = get_global_executor() if executor is None else executor - try: - executor.add_node(node) - executor.spin_until_future_complete(future, timeout_sec) - finally: - executor.remove_node(node) diff --git a/src/rclpy/action/__init__.py b/src/rclpy/action/__init__.py deleted file mode 100644 index 3fb5fbd..0000000 --- a/src/rclpy/action/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from .client import ActionClient # noqa: F401 -from .graph import get_action_client_names_and_types_by_node # noqa: F401 -from .graph import get_action_names_and_types # noqa -from .graph import get_action_server_names_and_types_by_node # noqa: F401 -from .server import ActionServer, CancelResponse, GoalResponse # noqa: F401 diff --git a/src/rclpy/action/client.py b/src/rclpy/action/client.py deleted file mode 100644 index cccc8b8..0000000 --- a/src/rclpy/action/client.py +++ /dev/null @@ -1,596 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import threading -import time -import uuid -import weakref - -from action_msgs.msg import GoalStatus -from action_msgs.srv import CancelGoal - -from rclpy.executors import await_or_execute -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import qos_profile_action_status_default -from rclpy.qos import qos_profile_services_default -from rclpy.qos import QoSProfile -from rclpy.task import Future -from rclpy.type_support import check_for_type_support -from rclpy.waitable import NumberOfEntities, Waitable - -from unique_identifier_msgs.msg import UUID - - -class ClientGoalHandle(): - """Goal handle for working with Action Clients.""" - - def __init__(self, action_client, goal_id, goal_response): - self._action_client = action_client - self._goal_id = goal_id - self._goal_response = goal_response - self._status = GoalStatus.STATUS_UNKNOWN - - def __eq__(self, other): - return self._goal_id == other.goal_id - - def __ne__(self, other): - return self._goal_id != other.goal_id - - def __repr__(self): - return 'ClientGoalHandle '.format( - self.goal_id.uuid, - self.accepted, - self.status) - - @property - def goal_id(self): - return self._goal_id - - @property - def stamp(self): - return self._goal_response.stamp - - @property - def accepted(self): - return self._goal_response.accepted - - @property - def status(self): - return self._status - - def cancel_goal(self): - """ - Send a cancel request for the goal and wait for the response. - - Do not call this method in a callback or a deadlock may occur. - - :return: The cancel response. - """ - return self._action_client._cancel_goal(self) - - def cancel_goal_async(self): - """ - Asynchronous request for the goal be canceled. - - :return: a Future instance that completes when the server responds. - :rtype: :class:`rclpy.task.Future` instance - """ - return self._action_client._cancel_goal_async(self) - - def get_result(self): - """ - Request the result for the goal and wait for the response. - - Do not call this method in a callback or a deadlock may occur. - - :return: The result response. - """ - return self._action_client._get_result(self) - - def get_result_async(self): - """ - Asynchronously request the goal result. - - :return: a Future instance that completes when the result is ready. - :rtype: :class:`rclpy.task.Future` instance - """ - return self._action_client._get_result_async(self) - - -class ActionClient(Waitable): - """ROS Action client.""" - - def __init__( - self, - node, - action_type, - 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 - ): - """ - Create an ActionClient. - - :param node: The ROS node to add the action client to. - :param action_type: Type of the action. - :param action_name: Name of the action. - Used as part of the underlying topic and service names. - :param callback_group: Callback group to add the action client to. - If None, then the node's default callback group is used. - :param goal_service_qos_profile: QoS profile for the goal service. - :param result_service_qos_profile: QoS profile for the result service. - :param cancel_service_qos_profile: QoS profile for the cancel service. - :param feedback_sub_qos_profile: QoS profile for the feedback subscriber. - :param status_sub_qos_profile: QoS profile for the status subscriber. - """ - if callback_group is None: - callback_group = node.default_callback_group - - super().__init__(callback_group) - - # Import the typesupport for the action module if not already done - check_for_type_support(action_type) - self._node = node - self._action_type = action_type - self._action_name = action_name - with node.handle: - self._client_handle = _rclpy.ActionClient( - node.handle, - action_type, - action_name, - goal_service_qos_profile.get_c_qos_profile(), - result_service_qos_profile.get_c_qos_profile(), - cancel_service_qos_profile.get_c_qos_profile(), - feedback_sub_qos_profile.get_c_qos_profile(), - status_sub_qos_profile.get_c_qos_profile() - ) - - self._is_ready = False - - # key: UUID in bytes, value: weak reference to ClientGoalHandle - self._goal_handles = {} - # key: goal request sequence_number, value: Future for goal response - self._pending_goal_requests = {} - # key: goal request sequence_number, value: UUID - self._goal_sequence_number_to_goal_id = {} - # key: cancel request sequence number, value: Future for cancel response - self._pending_cancel_requests = {} - # key: result request sequence number, value: Future for result response - self._pending_result_requests = {} - # key: result request sequence_number, value: UUID - self._result_sequence_number_to_goal_id = {} - # key: UUID in bytes, value: callback function - self._feedback_callbacks = {} - - callback_group.add_entity(self) - self._node.add_waitable(self) - - def _generate_random_uuid(self): - return UUID(uuid=list(uuid.uuid4().bytes)) - - def _remove_pending_request(self, future, pending_requests): - """ - Remove a future from the list of pending requests. - - This prevents a future from receiving a request and executing its done callbacks. - :param future: a future returned from one of :meth:`send_goal_async`, - :meth:`_cancel_goal_async`, or :meth:`_get_result_async`. - :type future: rclpy.task.Future - :param pending_requests: The list of pending requests. - :type pending_requests: dict - :return: The sequence number associated with the removed future, or - None if the future was not found in the list. - """ - for seq, req_future in list(pending_requests.items()): - if future == req_future: - try: - del pending_requests[seq] - except KeyError: - pass - else: - self.remove_future(future) - return seq - return None - - def _remove_pending_goal_request(self, future): - seq = self._remove_pending_request(future, self._pending_goal_requests) - if seq in self._goal_sequence_number_to_goal_id: - del self._goal_sequence_number_to_goal_id[seq] - - def _remove_pending_cancel_request(self, future): - self._remove_pending_request(future, self._pending_cancel_requests) - - def _remove_pending_result_request(self, future): - seq = self._remove_pending_request(future, self._pending_result_requests) - if seq in self._result_sequence_number_to_goal_id: - goal_uuid = bytes(self._result_sequence_number_to_goal_id[seq].uuid) - del self._result_sequence_number_to_goal_id[seq] - # remove feeback_callback if user is aware of result and it's been received - if goal_uuid in self._feedback_callbacks: - del self._feedback_callbacks[goal_uuid] - - # Start Waitable API - def is_ready(self, wait_set): - """Return True if one or more entities are ready in the wait set.""" - ready_entities = self._client_handle.is_ready(wait_set) - self._is_feedback_ready = ready_entities[0] - self._is_status_ready = ready_entities[1] - self._is_goal_response_ready = ready_entities[2] - self._is_cancel_response_ready = ready_entities[3] - self._is_result_response_ready = ready_entities[4] - return any(ready_entities) - - 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 = self._client_handle.take_goal_response( - 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 = self._client_handle.take_cancel_response( - 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 = self._client_handle.take_result_response( - 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 = self._client_handle.take_feedback( - 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 = self._client_handle.take_status( - self._action_type.Impl.GoalStatusMessage) - # If take fails, then we get None - if taken_data is not None: - data['status'] = taken_data - - return data - - async def execute(self, taken_data): - """ - Execute work after data has been taken from a ready wait set. - - This will set results for Future objects for any received service responses and - call any user-defined callbacks (e.g. feedback). - """ - if 'goal' in taken_data: - sequence_number, goal_response = taken_data['goal'] - if sequence_number in self._goal_sequence_number_to_goal_id: - goal_handle = ClientGoalHandle( - self, - self._goal_sequence_number_to_goal_id[sequence_number], - goal_response) - - if goal_handle.accepted: - goal_uuid = bytes(goal_handle.goal_id.uuid) - if goal_uuid in self._goal_handles: - raise RuntimeError( - 'Two goals were accepted with the same ID ({})'.format(goal_handle)) - self._goal_handles[goal_uuid] = weakref.ref(goal_handle) - - self._pending_goal_requests[sequence_number].set_result(goal_handle) - else: - self._node.get_logger().warning( - 'Ignoring unexpected goal response. There may be more than ' - f"one action server for the action '{self._action_name}'" - ) - - if 'cancel' in taken_data: - sequence_number, cancel_response = taken_data['cancel'] - if sequence_number in self._pending_cancel_requests: - self._pending_cancel_requests[sequence_number].set_result(cancel_response) - else: - self._node.get_logger().warning( - 'Ignoring unexpected cancel response. There may be more than ' - f"one action server for the action '{self._action_name}'" - ) - - if 'result' in taken_data: - sequence_number, result_response = taken_data['result'] - if sequence_number in self._pending_result_requests: - self._pending_result_requests[sequence_number].set_result(result_response) - else: - self._node.get_logger().warning( - 'Ignoring unexpected result response. There may be more than ' - f"one action server for the action '{self._action_name}'" - ) - - if 'feedback' in taken_data: - feedback_msg = taken_data['feedback'] - goal_uuid = bytes(feedback_msg.goal_id.uuid) - # Call a registered callback if there is one - if goal_uuid in self._feedback_callbacks: - await await_or_execute(self._feedback_callbacks[goal_uuid], feedback_msg) - - if 'status' in taken_data: - # Update the status of all goal handles maintained by this Action Client - for status_msg in taken_data['status'].status_list: - goal_uuid = bytes(status_msg.goal_info.goal_id.uuid) - status = status_msg.status - - if goal_uuid in self._goal_handles: - goal_handle = self._goal_handles[goal_uuid]() - if goal_handle is not None: - goal_handle._status = status - # Remove "done" goals from the list - if (GoalStatus.STATUS_SUCCEEDED == status or - GoalStatus.STATUS_CANCELED == status or - GoalStatus.STATUS_ABORTED == status): - del self._goal_handles[goal_uuid] - else: - # Weak reference is None - del self._goal_handles[goal_uuid] - - def get_num_entities(self): - """Return number of each type of entity used in the wait set.""" - num_entities = self._client_handle.get_num_entities() - return NumberOfEntities(*num_entities) - - def add_to_wait_set(self, wait_set): - """Add entities to wait set.""" - self._client_handle.add_to_waitset(wait_set) - - def __enter__(self): - return self._client_handle.__enter__() - - def __exit__(self, t, v, tb): - self._client_handle.__exit__(t, v, tb) - - # End Waitable API - - def send_goal(self, goal, **kwargs): - """ - Send a goal and wait for the result. - - Do not call this method in a callback or a deadlock may occur. - - See :meth:`send_goal_async` for more info about keyword arguments. - - Unlike :meth:`send_goal_async`, this method returns the final result of the - action (not a goal handle). - - :param goal: The goal request. - :type goal: action_type.Goal - :return: The result response. - :rtype: action_type.Result - :raises: TypeError if the type of the passed goal isn't an instance of - the Goal type of the provided action when the service was - constructed. - """ - if not isinstance(goal, self._action_type.Goal): - raise TypeError() - - event = threading.Event() - - def unblock(future): - nonlocal event - event.set() - - send_goal_future = self.send_goal_async(goal, **kwargs) - send_goal_future.add_done_callback(unblock) - - event.wait() - if send_goal_future.exception() is not None: - raise send_goal_future.exception() - - goal_handle = send_goal_future.result() - - result = self._get_result(goal_handle) - - return result - - def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): - """ - Send a goal and asynchronously get the result. - - The result of the returned Future is set to a ClientGoalHandle when receipt of the goal - is acknowledged by an action server. - - :param goal: The goal request. - :type goal: action_type.Goal - :param feedback_callback: Callback function for feedback associated with the goal. - :type feedback_callback: function - :param goal_uuid: Universally unique identifier for the goal. - If None, then a random UUID is generated. - :type: unique_identifier_msgs.UUID - :return: a Future instance to a goal handle that completes when the goal request - has been accepted or rejected. - :rtype: :class:`rclpy.task.Future` instance - :raises: TypeError if the type of the passed goal isn't an instance of - the Goal type of the provided action when the service was - constructed. - """ - if not isinstance(goal, self._action_type.Goal): - raise TypeError() - - 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 = self._client_handle.send_goal_request(request) - if sequence_number in self._pending_goal_requests: - raise RuntimeError( - 'Sequence ({}) conflicts with pending goal request'.format(sequence_number)) - - if feedback_callback is not None: - # TODO(jacobperron): Move conversion function to a general-use package - goal_uuid = bytes(request.goal_id.uuid) - self._feedback_callbacks[goal_uuid] = feedback_callback - - future = Future() - self._pending_goal_requests[sequence_number] = future - self._goal_sequence_number_to_goal_id[sequence_number] = request.goal_id - future.add_done_callback(self._remove_pending_goal_request) - # Add future so executor is aware - self.add_future(future) - - return future - - def _cancel_goal(self, goal_handle): - """ - Send a cancel request for an active goal and wait for the response. - - Do not call this method in a callback or a deadlock may occur. - - :param goal_handle: Handle to the goal to cancel. - :type goal_handle: :class:`ClientGoalHandle` - :return: The cancel response. - """ - event = threading.Event() - - def unblock(future): - nonlocal event - event.set() - - future = self._cancel_goal_async(goal_handle) - future.add_done_callback(unblock) - - event.wait() - if future.exception() is not None: - raise future.exception() - return future.result() - - def _cancel_goal_async(self, goal_handle): - """ - Send a cancel request for an active goal and asynchronously get the result. - - :param goal_handle: Handle to the goal to cancel. - :type goal_handle: :class:`ClientGoalHandle` - :return: a Future instance that completes when the cancel request has been processed. - :rtype: :class:`rclpy.task.Future` instance - """ - if not isinstance(goal_handle, ClientGoalHandle): - raise TypeError( - 'Expected type ClientGoalHandle but received {}'.format(type(goal_handle))) - - cancel_request = CancelGoal.Request() - cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = self._client_handle.send_cancel_request(cancel_request) - if sequence_number in self._pending_cancel_requests: - raise RuntimeError( - 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) - - future = Future() - self._pending_cancel_requests[sequence_number] = future - future.add_done_callback(self._remove_pending_cancel_request) - # Add future so executor is aware - self.add_future(future) - - return future - - def _get_result(self, goal_handle): - """ - Request the result for an active goal and wait for the response. - - Do not call this method in a callback or a deadlock may occur. - - :param goal_handle: Handle to the goal to get the result for. - :type goal_handle: :class:`ClientGoalHandle` - :return: The result response. - """ - event = threading.Event() - - def unblock(future): - nonlocal event - event.set() - - future = self._get_result_async(goal_handle) - future.add_done_callback(unblock) - - event.wait() - if future.exception() is not None: - raise future.exception() - return future.result() - - def _get_result_async(self, goal_handle): - """ - Request the result for an active goal asynchronously. - - :param goal_handle: Handle to the goal to cancel. - :type goal_handle: :class:`ClientGoalHandle` - :return: a Future instance that completes when the get result request has been processed. - :rtype: :class:`rclpy.task.Future` instance - """ - if not isinstance(goal_handle, ClientGoalHandle): - raise TypeError( - 'Expected type ClientGoalHandle but received {}'.format(type(goal_handle))) - - result_request = self._action_type.Impl.GetResultService.Request() - result_request.goal_id = goal_handle.goal_id - sequence_number = self._client_handle.send_result_request(result_request) - if sequence_number in self._pending_result_requests: - raise RuntimeError( - 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) - - future = Future() - self._pending_result_requests[sequence_number] = future - self._result_sequence_number_to_goal_id[sequence_number] = result_request.goal_id - future.add_done_callback(self._remove_pending_result_request) - # Add future so executor is aware - self.add_future(future) - - return future - - def server_is_ready(self): - """ - Check if there is an action server ready to process requests from this client. - - :return: True if an action server is ready, False otherwise. - """ - with self._node.handle: - return self._client_handle.is_action_server_available() - - def wait_for_server(self, timeout_sec=None): - """ - Wait for an action sever to be ready. - - Returns as soon as an action server is ready for this client. - - :param timeout_sec: Number of seconds to wait until an action server is available. - If None, then wait indefinitely. - :return: True if an action server is available, False if the timeout is exceeded. - """ - # TODO(jacobperron): Remove arbitrary sleep time and return as soon as server is ready - # See https://github.com/ros2/rclpy/issues/58 - sleep_time = 0.25 - if timeout_sec is None: - timeout_sec = float('inf') - while self._node.context.ok() and not self.server_is_ready() and timeout_sec > 0.0: - time.sleep(sleep_time) - timeout_sec -= sleep_time - - return self.server_is_ready() - - def destroy(self): - """Destroy the underlying action client handle.""" - self._client_handle.destroy_when_not_in_use() - self._node.remove_waitable(self) diff --git a/src/rclpy/action/graph.py b/src/rclpy/action/graph.py deleted file mode 100644 index 362e48b..0000000 --- a/src/rclpy/action/graph.py +++ /dev/null @@ -1,71 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import List -from typing import Tuple - -from rclpy.node import Node - - -def get_action_client_names_and_types_by_node( - node: Node, - remote_node_name: str, - remote_node_namespace: str -) -> List[Tuple[str, List[str]]]: - """ - Get a list of action names and types for action clients associated with a node. - - :param node: The node used for discovery. - :param remote_node_name: The name of a remote node to get action clients for. - :param node_namespace: Namespace of the remote node. - :return: List of tuples. - The first element of each tuple is the action name and the second element is a list of - action types. - """ - with node.handle: - return node.handle.get_action_client_names_and_types_by_node( - remote_node_name, remote_node_namespace) - - -def get_action_server_names_and_types_by_node( - node: Node, - remote_node_name: str, - remote_node_namespace: str -) -> List[Tuple[str, List[str]]]: - """ - Get a list of action names and types for action servers associated with a node. - - :param node: The node used for discovery. - :param remote_node_name: The name of a remote node to get action servers for. - :param node_namespace: Namespace of the remote node. - :return: List of tuples. - The first element of each tuple is the action name and the second element is a list of - action types. - """ - with node.handle: - return node.handle.get_action_server_names_and_types_by_node( - remote_node_name, remote_node_namespace) - - -def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: - """ - Get a list of action names and types. - - :param node: The node used for discovery. - :return: List of action names and types in the ROS graph as tuples. - The first element of each tuple is the action name and the second element is a list of - action types. - """ - with node.handle: - return node.handle.get_action_names_and_types() diff --git a/src/rclpy/action/server.py b/src/rclpy/action/server.py deleted file mode 100644 index c10dbbd..0000000 --- a/src/rclpy/action/server.py +++ /dev/null @@ -1,631 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import Enum -import functools -import threading -import traceback - -from action_msgs.msg import GoalInfo, GoalStatus - -from rclpy.executors import await_or_execute -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import qos_profile_action_status_default -from rclpy.qos import qos_profile_services_default -from rclpy.qos import QoSProfile -from rclpy.task import Future -from rclpy.type_support import check_for_type_support -from rclpy.waitable import NumberOfEntities, Waitable - -# Re-export exception defined in _rclpy C extension. -RCLError = _rclpy.RCLError - - -class GoalResponse(Enum): - """Possible goal responses.""" - - REJECT = 1 - ACCEPT = 2 - - -class CancelResponse(Enum): - """Possible cancel responses.""" - - REJECT = 1 - ACCEPT = 2 - - -GoalEvent = _rclpy.GoalEvent - - -class ServerGoalHandle: - """Goal handle for working with Action Servers.""" - - def __init__(self, action_server, goal_info, goal_request): - """ - Accept a new goal with the given action server. - - Instances of this class should only be constructed in the ActionServer class. - Instances for accepted goals will be passed to the user-defined goal execution functions. - - If the goal fails to be accepted, then a RuntimeError is raised. - - :param action_server: The ActionServer to accept the goal. - :param goal_info: GoalInfo message. - :param goal_request: The user defined goal request message from an ActionClient. - """ - self._goal_handle = _rclpy.ActionGoalHandle(action_server._handle, goal_info) - self._action_server = action_server - self._goal_info = goal_info - self._goal_request = goal_request - self._cancel_requested = False - self._lock = threading.Lock() - - def __eq__(self, other): - return self.goal_id == other.goal_id - - def __ne__(self, other): - return self.goal_id != other.goal_id - - @property - def request(self): - return self._goal_request - - @property - def goal_id(self): - return self._goal_info.goal_id - - @property - def is_active(self): - with self._lock: - if self._goal_handle is None: - return False - return self._goal_handle.is_active() - - @property - def is_cancel_requested(self): - return GoalStatus.STATUS_CANCELING == self.status - - @property - def status(self): - with self._lock: - if self._goal_handle is None: - return GoalStatus.STATUS_UNKNOWN - return self._goal_handle.get_status() - - def _update_state(self, event): - with self._lock: - # Ignore updates for already destructed goal handles - if self._goal_handle is None: - return - - # Update state - self._goal_handle.update_goal_state(event) - - # Publish state change - self._action_server._handle.publish_status() - - # If it's a terminal state, then also notify the action server - if not self._goal_handle.is_active(): - self._action_server.notify_goal_done() - - def execute(self, execute_callback=None): - # It's possible that there has been a request to cancel the goal prior to executing. - # In this case we want to avoid the illegal state transition to EXECUTING - # but still call the users execute callback to let them handle canceling the goal. - if not self.is_cancel_requested: - self._update_state(_rclpy.GoalEvent.EXECUTE) - self._action_server.notify_execute(self, execute_callback) - - def publish_feedback(self, feedback): - if not isinstance(feedback, self._action_server.action_type.Feedback): - raise TypeError() - - with self._lock: - # Ignore for already destructed goal handles - if self._goal_handle is None: - return - - # Populate the feedback message with metadata about this goal - # and the user defined message - feedback_message = self._action_server.action_type.Impl.FeedbackMessage() - feedback_message.goal_id = self.goal_id - feedback_message.feedback = feedback - - # Publish - self._action_server._handle.publish_feedback(feedback_message) - - def succeed(self): - self._update_state(_rclpy.GoalEvent.SUCCEED) - - def abort(self): - self._update_state(_rclpy.GoalEvent.ABORT) - - def canceled(self): - self._update_state(_rclpy.GoalEvent.CANCELED) - - def destroy(self): - with self._lock: - if self._goal_handle is None: - return - self._goal_handle.destroy_when_not_in_use() - self._goal_handle = None - - -def default_handle_accepted_callback(goal_handle): - """Execute the goal.""" - goal_handle.execute() - - -def default_goal_callback(goal_request): - """Accept all goals.""" - return GoalResponse.ACCEPT - - -def default_cancel_callback(cancel_request): - """No cancellations.""" - return CancelResponse.REJECT - - -class ActionServer(Waitable): - """ROS Action server.""" - - def __init__( - self, - node, - action_type, - action_name, - execute_callback, - *, - callback_group=None, - goal_callback=default_goal_callback, - handle_accepted_callback=default_handle_accepted_callback, - cancel_callback=default_cancel_callback, - goal_service_qos_profile=qos_profile_services_default, - result_service_qos_profile=qos_profile_services_default, - cancel_service_qos_profile=qos_profile_services_default, - feedback_pub_qos_profile=QoSProfile(depth=10), - status_pub_qos_profile=qos_profile_action_status_default, - result_timeout=900 - ): - """ - Create an ActionServer. - - :param node: The ROS node to add the action server to. - :param action_type: Type of the action. - :param action_name: Name of the action. - Used as part of the underlying topic and service names. - :param execute_callback: Callback function for processing accepted goals. - This is called if when :class:`ServerGoalHandle.execute()` is called for - a goal handle that is being tracked by this action server. - :param callback_group: Callback group to add the action server to. - If None, then the node's default callback group is used. - :param goal_callback: Callback function for handling new goal requests. - :param handle_accepted_callback: Callback function for handling newly accepted goals. - Passes an instance of `ServerGoalHandle` as an argument. - :param cancel_callback: Callback function for handling cancel requests. - :param goal_service_qos_profile: QoS profile for the goal service. - :param result_service_qos_profile: QoS profile for the result service. - :param cancel_service_qos_profile: QoS profile for the cancel service. - :param feedback_pub_qos_profile: QoS profile for the feedback publisher. - :param status_pub_qos_profile: QoS profile for the status publisher. - :param result_timeout: How long in seconds a result is kept by the server after a goal - reaches a terminal state. - """ - if callback_group is None: - callback_group = node.default_callback_group - - super().__init__(callback_group) - - self._lock = threading.Lock() - - self.register_handle_accepted_callback(handle_accepted_callback) - self.register_goal_callback(goal_callback) - self.register_cancel_callback(cancel_callback) - self.register_execute_callback(execute_callback) - - # Import the typesupport for the action module if not already done - check_for_type_support(action_type) - self._node = node - self._action_type = action_type - with node.handle, node.get_clock().handle: - self._handle = _rclpy.ActionServer( - node.handle, - node.get_clock().handle, - action_type, - action_name, - goal_service_qos_profile.get_c_qos_profile(), - result_service_qos_profile.get_c_qos_profile(), - cancel_service_qos_profile.get_c_qos_profile(), - feedback_pub_qos_profile.get_c_qos_profile(), - status_pub_qos_profile.get_c_qos_profile(), - result_timeout, - ) - - # key: UUID in bytes, value: GoalHandle - self._goal_handles = {} - - # key: UUID in bytes, value: Future - self._result_futures = {} - - callback_group.add_entity(self) - self._node.add_waitable(self) - - async def _execute_goal_request(self, request_header_and_message): - request_header, goal_request = request_header_and_message - goal_uuid = goal_request.goal_id - goal_info = GoalInfo() - goal_info.goal_id = goal_uuid - - self._node.get_logger().debug('New goal request with ID: {0}'.format(goal_uuid.uuid)) - - # Check if goal ID is already being tracked by this action server - with self._lock: - goal_id_exists = self._handle.goal_exists(goal_info) - - accepted = False - if not goal_id_exists: - # Call user goal callback - response = await await_or_execute(self._goal_callback, goal_request.goal) - if not isinstance(response, GoalResponse): - self._node.get_logger().warning( - 'Goal request callback did not return a GoalResponse type. Rejecting goal.') - else: - accepted = GoalResponse.ACCEPT == response - - if accepted: - # Stamp time of acceptance - goal_info.stamp = self._node.get_clock().now().to_msg() - - # Create a goal handle - try: - with self._lock: - goal_handle = ServerGoalHandle(self, goal_info, goal_request.goal) - except RuntimeError as e: - self._node.get_logger().error( - 'Failed to accept new goal with ID {0}: {1}'.format(goal_uuid.uuid, e)) - accepted = False - else: - self._goal_handles[bytes(goal_uuid.uuid)] = goal_handle - self._result_futures[bytes(goal_uuid.uuid)] = Future() - self.add_future(self._result_futures[bytes(goal_uuid.uuid)]) - - # Send response - response_msg = self._action_type.Impl.SendGoalService.Response() - response_msg.accepted = accepted - response_msg.stamp = goal_info.stamp - - try: - # If the client goes away anytime before this, sending the goal response may fail. - # Catch the exception here and go on so we don't crash. - self._handle.send_goal_response(request_header, response_msg) - except RCLError: - self._node.get_logger().warn( - 'Failed to send goal response (the client may have gone away)') - return - - if not accepted: - self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid)) - return - - self._node.get_logger().debug('New goal accepted: {0}'.format(goal_uuid.uuid)) - - # Provide the user a reference to the goal handle - await await_or_execute(self._handle_accepted_callback, goal_handle) - - async def _execute_goal(self, execute_callback, goal_handle): - goal_uuid = goal_handle.goal_id.uuid - self._node.get_logger().debug('Executing goal with ID {0}'.format(goal_uuid)) - - try: - # Execute user callback - execute_result = await await_or_execute(execute_callback, goal_handle) - except Exception as ex: - # Create an empty result so that we can still send a response to the client - execute_result = self._action_type.Result() - self._node.get_logger().error('Error raised in execute callback: {0}'.format(ex)) - traceback.print_exc() - - # If user did not trigger a terminal state, assume aborted - if goal_handle.is_active: - self._node.get_logger().warning( - 'Goal state not set, assuming aborted. Goal ID: {0}'.format(goal_uuid)) - goal_handle.abort() - - self._node.get_logger().debug( - 'Goal with ID {0} finished with state {1}'.format(goal_uuid, goal_handle.status)) - - # Set result - result_response = self._action_type.Impl.GetResultService.Response() - result_response.status = goal_handle.status - result_response.result = execute_result - self._result_futures[bytes(goal_uuid)].set_result(result_response) - - async def _execute_cancel_request(self, request_header_and_message): - request_header, cancel_request = request_header_and_message - - self._node.get_logger().debug('Cancel request received: {0}'.format(cancel_request)) - - with self._lock: - # Get list of goals that are requested to be canceled - cancel_response = self._handle.process_cancel_request( - cancel_request, self._action_type.Impl.CancelGoalService.Response) - - for goal_info in cancel_response.goals_canceling: - goal_uuid = bytes(goal_info.goal_id.uuid) - if goal_uuid not in self._goal_handles: - # Possibly the user doesn't care to track the goal handle - # Remove from response - cancel_response.goals_canceling.remove(goal_info) - continue - goal_handle = self._goal_handles[goal_uuid] - response = await await_or_execute(self._cancel_callback, goal_handle) - - if CancelResponse.ACCEPT == response: - # Notify goal handle - try: - # If the goal's just succeeded after user cancel callback - # that will generate an exception from invalid transition. - goal_handle._update_state(GoalEvent.CANCEL_GOAL) - except RCLError as ex: - self._node.get_logger().debug( - 'Failed to cancel goal in cancel callback: {0}'.format(ex)) - # Remove from response since goal has been succeeded - cancel_response.goals_canceling.remove(goal_info) - else: - # Remove from response - cancel_response.goals_canceling.remove(goal_info) - - try: - # If the client goes away anytime before this, sending the goal response may fail. - # Catch the exception here and go on so we don't crash. - self._handle.send_cancel_response(request_header, cancel_response) - except RCLError: - self._node.get_logger().warn( - 'Failed to send cancel response (the client may have gone away)') - - async def _execute_get_result_request(self, request_header_and_message): - request_header, result_request = request_header_and_message - goal_uuid = result_request.goal_id.uuid - - self._node.get_logger().debug( - 'Result request received for goal with ID: {0}'.format(goal_uuid)) - - # If no goal with the requested ID exists, then return UNKNOWN status - if bytes(goal_uuid) not in self._goal_handles: - self._node.get_logger().debug( - '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 - self._handle.send_result_response(request_header, result_response) - return - - # There is an accepted goal matching the goal ID, register a callback to send the - # response as soon as it's ready - self._result_futures[bytes(goal_uuid)].add_done_callback( - functools.partial(self._send_result_response, request_header)) - - async def _execute_expire_goals(self, expired_goals): - for goal in expired_goals: - goal_uuid = bytes(goal.goal_id.uuid) - self._goal_handles[goal_uuid].destroy() - del self._goal_handles[goal_uuid] - self.remove_future(self._result_futures[goal_uuid]) - del self._result_futures[goal_uuid] - - def _send_result_response(self, request_header, future): - try: - # If the client goes away anytime before this, sending the result response may fail. - # Catch the exception here and go on so we don't crash. - self._handle.send_result_response(request_header, future.result()) - except RCLError: - self._node.get_logger().warn( - 'Failed to send result response (the client may have gone away)') - - @property - def action_type(self): - return self._action_type - - # Start Waitable API - def is_ready(self, wait_set): - """Return True if one or more entities are ready in the wait set.""" - with self._lock: - ready_entities = self._handle.is_ready(wait_set) - self._is_goal_request_ready = ready_entities[0] - self._is_cancel_request_ready = ready_entities[1] - self._is_result_request_ready = ready_entities[2] - self._is_goal_expired = ready_entities[3] - return any(ready_entities) - - def take_data(self): - """Take stuff from lower level so the wait set doesn't immediately wake again.""" - data = {} - if self._is_goal_request_ready: - with self._lock: - taken_data = self._handle.take_goal_request( - self._action_type.Impl.SendGoalService.Request, - ) - # If take fails, then we get (None, None) - if all(taken_data): - data['goal'] = taken_data - - if self._is_cancel_request_ready: - with self._lock: - taken_data = self._handle.take_cancel_request( - self._action_type.Impl.CancelGoalService.Request, - ) - # If take fails, then we get (None, None) - if all(taken_data): - data['cancel'] = taken_data - - if self._is_result_request_ready: - with self._lock: - taken_data = self._handle.take_result_request( - self._action_type.Impl.GetResultService.Request, - ) - # If take fails, then we get (None, None) - if all(taken_data): - data['result'] = taken_data - - if self._is_goal_expired: - with self._lock: - data['expired'] = self._handle.expire_goals(len(self._goal_handles)) - - return data - - async def execute(self, taken_data): - """ - Execute work after data has been taken from a ready wait set. - - This will set results for Future objects for any received service responses and - call any user-defined callbacks (e.g. feedback). - """ - if 'goal' in taken_data: - await self._execute_goal_request(taken_data['goal']) - - if 'cancel' in taken_data: - await self._execute_cancel_request(taken_data['cancel']) - - if 'result' in taken_data: - await self._execute_get_result_request(taken_data['result']) - - if 'expired' in taken_data: - await self._execute_expire_goals(taken_data['expired']) - - def get_num_entities(self): - """Return number of each type of entity used in the wait set.""" - num_entities = self._handle.get_num_entities() - return NumberOfEntities( - num_entities[0], - num_entities[1], - num_entities[2], - num_entities[3], - num_entities[4]) - - def add_to_wait_set(self, wait_set): - """Add entities to wait set.""" - with self._lock: - self._handle.add_to_waitset(wait_set) - - def __enter__(self): - return self._handle.__enter__() - - def __exit__(self, t, v, tb): - self._handle.__exit__(t, v, tb) - - # End Waitable API - - def notify_execute(self, goal_handle, execute_callback): - # Use provided callback, defaulting to a previously registered callback - if execute_callback is None: - if self._execute_callback is None: - return - execute_callback = self._execute_callback - - # Schedule user callback for execution - self._node.executor.create_task(self._execute_goal, execute_callback, goal_handle) - - def notify_goal_done(self): - with self._lock: - self._handle.notify_goal_done() - - def register_handle_accepted_callback(self, handle_accepted_callback): - """ - Register a callback for handling newly accepted goals. - - The provided function is called whenever a new goal has been accepted by this - action server. - The function should expect an instance of :class:`ServerGoalHandle` as an argument, which - represents a handle to the goal that was accepted. - The goal handle can be used to interact with the goal, e.g. publish feedback, - update the status, or execute a deferred goal. - - There can only be one handle accepted callback per :class:`ActionServer`, therefore - calling this function will replace any previously registered callback. - - :param goal_callback: Callback function, if `None`, then unregisters any previously - registered callback. - """ - if handle_accepted_callback is None: - handle_accepted_callback = default_handle_accepted_callback - self._handle_accepted_callback = handle_accepted_callback - - def register_goal_callback(self, goal_callback): - """ - Register a callback for handling new goal requests. - - The purpose of the goal callback is to decide if a new goal should be accepted or - rejected. - The callback should take the goal request message as a parameter and must return a - :class:`GoalResponse` value. - - There can only be one goal callback per :class:`ActionServer`, therefore calling this - function will replace any previously registered callback. - - :param goal_callback: Callback function, if `None`, then unregisters any previously - registered callback. - """ - if goal_callback is None: - goal_callback = default_goal_callback - self._goal_callback = goal_callback - - def register_cancel_callback(self, cancel_callback): - """ - Register a callback for handling cancel requests. - - The purpose of the cancel callback is to decide if a request to cancel an on-going - (or queued) goal should be accepted or rejected. - The callback should take one parameter containing the cancel request and must return a - :class:`CancelResponse` value. - - There can only be one cancel callback per :class:`ActionServer`, therefore calling this - function will replace any previously registered callback. - - :param cancel_callback: Callback function, if `None`, then unregisters any previously - registered callback. - """ - if cancel_callback is None: - cancel_callback = default_cancel_callback - self._cancel_callback = cancel_callback - - def register_execute_callback(self, execute_callback): - """ - Register a callback for executing action goals. - - The purpose of the execute callback is to execute the action goal and return a result - when finished. - - The callback should take one parameter containing goal request and must return a - result instance (i.e. `action_type.Result`). - - There can only be one execute callback per :class:`ActionServer`, therefore calling this - function will replace any previously registered callback. - - :param execute_callback: Callback function. Must not be `None`. - """ - if not callable(execute_callback): - raise TypeError('Failed to register goal execution callback: not callable') - self._execute_callback = execute_callback - - def destroy(self): - """Destroy the underlying action server handle.""" - for goal_handle in self._goal_handles.values(): - goal_handle.destroy() - - """Remove the underlying result future.""" - for result_future in self._result_futures.values(): - self.remove_future(result_future) - - self._handle.destroy_when_not_in_use() - self._node.remove_waitable(self) diff --git a/src/rclpy/callback_groups.py b/src/rclpy/callback_groups.py deleted file mode 100644 index ff98c2f..0000000 --- a/src/rclpy/callback_groups.py +++ /dev/null @@ -1,117 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from threading import Lock -import weakref - - -class CallbackGroup: - """ - The base class for a callback group. - - A callback group controls when callbacks are allowed to be executed. - - This class should not be instantiated. - Instead, classes should extend it and implement :meth:`can_execute`, - :meth:`beginning_execution`, and :meth:`ending_execution`. - """ - - def __init__(self) -> None: - super().__init__() - self.entities: set = set() - - def add_entity(self, entity) -> None: - """ - Add an entity to the callback group. - - :param entity: a subscription, timer, client, service, or waitable instance. - """ - self.entities.add(weakref.ref(entity)) - - def has_entity(self, entity) -> bool: - """ - Determine if an entity has been added to this group. - - :param entity: a subscription, timer, client, service, or waitable instance. - """ - return weakref.ref(entity) in self.entities - - def can_execute(self, entity) -> bool: - """ - Determine if an entity can be executed. - - :param entity: a subscription, timer, client, service, or waitable instance. - :return: ``True`` if the entity can be executed, ``False`` otherwise. - """ - raise NotImplementedError() - - def beginning_execution(self, entity) -> bool: - """ - Get permission for the callback from the group to begin executing an entity. - - If this returns ``True`` then :meth:`CallbackGroup.ending_execution` must be called after - the callback has been executed. - - :param entity: a subscription, timer, client, service, or waitable instance. - :return: ``True`` if the callback can be executed, ``False`` otherwise. - """ - raise NotImplementedError() - - def ending_execution(self, entity) -> None: - """ - Notify group that a callback has finished executing. - - :param entity: a subscription, timer, client, service, or waitable instance. - """ - raise NotImplementedError() - - -class ReentrantCallbackGroup(CallbackGroup): - """Allow callbacks to be executed in parallel without restriction.""" - - def can_execute(self, entity): - return True - - def beginning_execution(self, entity): - return True - - def ending_execution(self, entity): - pass - - -class MutuallyExclusiveCallbackGroup(CallbackGroup): - """Allow only one callback to be executing at a time.""" - - def __init__(self): - super().__init__() - self._active_entity = None - self._lock = Lock() - - def can_execute(self, entity): - with self._lock: - assert weakref.ref(entity) in self.entities - return self._active_entity is None - - def beginning_execution(self, entity): - with self._lock: - assert weakref.ref(entity) in self.entities - if self._active_entity is None: - self._active_entity = entity - return True - return False - - def ending_execution(self, entity): - with self._lock: - assert self._active_entity == entity - self._active_entity = None diff --git a/src/rclpy/client.py b/src/rclpy/client.py deleted file mode 100644 index 4e5528f..0000000 --- a/src/rclpy/client.py +++ /dev/null @@ -1,187 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import threading -import time -from typing import Dict -from typing import TypeVar - -from rclpy.callback_groups import CallbackGroup -from rclpy.context import Context -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import QoSProfile -from rclpy.task import Future - -# Used for documentation purposes only -SrvType = TypeVar('SrvType') -SrvTypeRequest = TypeVar('SrvTypeRequest') -SrvTypeResponse = TypeVar('SrvTypeResponse') - - -class Client: - def __init__( - self, - context: Context, - client_impl: _rclpy.Client, - srv_type: SrvType, - srv_name: str, - qos_profile: QoSProfile, - callback_group: CallbackGroup - ) -> None: - """ - Create a container for a ROS service client. - - .. warning:: Users should not create a service client with this constuctor, instead they - should call :meth:`.Node.create_client`. - - :param context: The context associated with the service client. - :param client_impl: :class:`_rclpy.Client` wrapping the underlying ``rcl_client_t`` object. - :param srv_type: The service type. - :param srv_name: The name of the service. - :param qos_profile: The quality of service profile to apply the service client. - :param callback_group: The callback group for the service client. If ``None``, then the - nodes default callback group is used. - """ - self.context = context - self.__client = client_impl - self.srv_type = srv_type - self.srv_name = srv_name - self.qos_profile = qos_profile - # Key is a sequence number, value is an instance of a Future - self._pending_requests: Dict[int, Future] = {} - self.callback_group = callback_group - # True when the callback is ready to fire but has not been "taken" by an executor - self._executor_event = False - - self._lock = threading.Lock() - - def call(self, request: SrvTypeRequest) -> SrvTypeResponse: - """ - Make a service request and wait for the result. - - .. warning:: Do not call this method in a callback or a deadlock may occur. - - :param request: The service request. - :return: The service response. - :raises: TypeError if the type of the passed request isn't an instance - of the Request type of the provided service when the client was - constructed. - """ - if not isinstance(request, self.srv_type.Request): - raise TypeError() - - event = threading.Event() - - def unblock(future): - nonlocal event - event.set() - - future = self.call_async(request) - future.add_done_callback(unblock) - - # Check future.done() before waiting on the event. - # The callback might have been added after the future is completed, - # resulting in the event never being set. - if not future.done(): - event.wait() - if future.exception() is not None: - raise future.exception() - return future.result() - - def call_async(self, request: SrvTypeRequest) -> Future: - """ - Make a service request and asyncronously get the result. - - :param request: The service request. - :return: A future that completes when the request does. - :raises: TypeError if the type of the passed request isn't an instance - of the Request type of the provided service when the client was - constructed. - """ - if not isinstance(request, self.srv_type.Request): - raise TypeError() - - with self._lock: - with self.handle: - sequence_number = self.__client.send_request(request) - if sequence_number in self._pending_requests: - raise RuntimeError(f'Sequence ({sequence_number}) conflicts with pending request') - - future = Future() - self._pending_requests[sequence_number] = future - - future.add_done_callback(self.remove_pending_request) - - return future - - def get_pending_request(self, sequence_number: int) -> Future: - """ - Get a future from the list of pending requests. - - :param sequence_number: Number identifying the pending request. - :return: The future corresponding to the sequence_number. - :raises: KeyError if the sequence_number is not in the pending requests. - """ - with self._lock: - return self._pending_requests[sequence_number] - - def remove_pending_request(self, future: Future) -> None: - """ - Remove a future from the list of pending requests. - - This prevents a future from receiving a response and executing its done callbacks. - - :param future: A future returned from :meth:`call_async` - """ - with self._lock: - for seq, req_future in self._pending_requests.items(): - if future is req_future: - del self._pending_requests[seq] - break - - def service_is_ready(self) -> bool: - """ - Check if there is a service server ready. - - :return: ``True`` if a server is ready, ``False`` otherwise. - """ - with self.handle: - return self.__client.service_server_is_available() - - def wait_for_service(self, timeout_sec: float = None) -> bool: - """ - Wait for a service server to become ready. - - Returns as soon as a server becomes ready or if the timeout expires. - - :param timeout_sec: Seconds to wait. If ``None``, then wait forever. - :return: ``True`` if server became ready while waiting or ``False`` on a timeout. - """ - # TODO(sloretz) Return as soon as the service is available - # This is a temporary implementation. The sleep time is arbitrary. - sleep_time = 0.25 - if timeout_sec is None: - timeout_sec = float('inf') - while self.context.ok() and not self.service_is_ready() and timeout_sec > 0.0: - time.sleep(sleep_time) - timeout_sec -= sleep_time - - return self.service_is_ready() - - @property - def handle(self): - return self.__client - - def destroy(self): - self.__client.destroy_when_not_in_use() diff --git a/src/rclpy/clock.py b/src/rclpy/clock.py deleted file mode 100644 index 0875adb..0000000 --- a/src/rclpy/clock.py +++ /dev/null @@ -1,284 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - -from .duration import Duration -from .exceptions import NotInitializedException -from .time import Time -from .utilities import get_default_context - - -ClockType = _rclpy.ClockType -ClockChange = _rclpy.ClockChange - - -class JumpThreshold: - - def __init__(self, *, min_forward: Duration, min_backward: Duration, on_clock_change=True): - """ - Initialize an instance of JumpThreshold. - - :param min_forward: Minimum jump forwards to be considered exceeded, or None. - The min_forward threshold is enabled only when given a positive Duration. - The duration must be positive, and not zero. - :param min_backward: Negative duration indicating minimum jump backwards to be considered - exceeded, or None. - The min_backward threshold enabled only when given a negative Duration. - The duration must be negative, and not zero. - :param on_clock_change: True to make a callback happen when ROS_TIME is activated - or deactivated. - """ - if min_forward is not None and min_forward.nanoseconds <= 0: - raise ValueError('min_forward must be a positive non-zero duration') - - if min_backward is not None and min_backward.nanoseconds >= 0: - raise ValueError('min_backward must be a negative non-zero duration') - - if min_forward is None and min_backward is None and not on_clock_change: - raise ValueError('At least one jump threshold must be enabled') - - self.min_forward = min_forward - self.min_backward = min_backward - self.on_clock_change = on_clock_change - - -class TimeJump: - - def __init__(self, clock_change, delta): - if not isinstance(clock_change, ClockChange): - raise TypeError('clock_change must be an instance of rclpy.clock.ClockChange') - self._clock_change = clock_change - self._delta = delta - - @property - def clock_change(self): - return self._clock_change - - @property - def delta(self): - return self._delta - - -class JumpHandle: - - def __init__(self, *, clock, threshold, pre_callback, post_callback): - """ - Register a clock jump callback. - - :param clock: Clock that time jump callback is registered to - :param pre_callback: Callback to be called before new time is set. - :param post_callback: Callback to be called after new time is set, accepting a dictionary - with keys "clock_change" and "delta". - """ - if pre_callback is None and post_callback is None: - raise ValueError('One of pre_callback or post_callback must be callable') - if pre_callback is not None and not callable(pre_callback): - raise ValueError('pre_callback must be callable if given') - if post_callback is not None and not callable(post_callback): - raise ValueError('post_callback must be callable if given') - self._clock = clock - self._pre_callback = pre_callback - self._post_callback = post_callback - - min_forward = 0 - if threshold.min_forward is not None: - min_forward = threshold.min_forward.nanoseconds - min_backward = 0 - if threshold.min_backward is not None: - min_backward = threshold.min_backward.nanoseconds - - with self._clock.handle: - self._clock.handle.add_clock_callback( - 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: - self._clock.handle.remove_clock_callback(self) - self._clock = None - - def __enter__(self): - return self - - def __exit__(self, t, v, tb): - self.unregister() - - -class Clock: - - def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): - if not isinstance(clock_type, ClockType): - raise TypeError('Clock type must be a ClockType enum') - if clock_type is ClockType.ROS_TIME: - self = super().__new__(ROSClock) - else: - self = super().__new__(cls) - self.__clock = _rclpy.Clock(clock_type) - self._clock_type = clock_type - return self - - @property - def clock_type(self): - return self._clock_type - - @property - def handle(self): - return self.__clock - - def __repr__(self): - return 'Clock(clock_type={0})'.format(self.clock_type.name) - - def now(self) -> Time: - with self.handle: - rcl_time = self.__clock.get_now() - return Time(nanoseconds=rcl_time.nanoseconds, clock_type=self.clock_type) - - def create_jump_callback(self, threshold, *, pre_callback=None, post_callback=None): - """ - Create callback handler for clock time jumps. - - The callbacks must remain valid as long as the returned JumpHandler is valid. - A callback should execute as quick as possible and must not block when called. - If a callback raises then no time jump callbacks added after it will be called. - - :param threshold: Criteria for activating time jump. - :param pre_callback: Callback to be called before new time is set. - :param post_callback: Callback to be called after new time is set accepting - :rtype: :class:`rclpy.clock.JumpHandler` - """ - if post_callback is not None and callable(post_callback): - original_callback = post_callback - - def callback_shim(jump_dict): - nonlocal original_callback - clock_change = jump_dict['clock_change'] - duration = Duration(nanoseconds=jump_dict['delta']) - original_callback(TimeJump(clock_change, duration)) - - post_callback = callback_shim - - return JumpHandle( - clock=self, threshold=threshold, pre_callback=pre_callback, - post_callback=post_callback) - - def sleep_until(self, until: Time, context=None) -> bool: - """ - Sleep until a Time on this Clock is reached. - - When using a ROSClock, this may sleep forever if the TimeSource is misconfigured and the - context is never shut down. - ROS time being activated or deactivated causes this function to cease sleeping and return - False. - - :param until: Time at which this function should stop sleeping. - :param context: Context which when shut down will cause this sleep to wake early. - If context is None, then the default context is used. - :return: True if until was reached, or False if it woke for another reason. - :raises ValueError: until is specified for a different type of clock than this one. - :raises NotInitializedException: context has not been initialized or is shutdown. - """ - if context is None: - context = get_default_context() - - if not context.ok(): - raise NotInitializedException() - - if until.clock_type != self._clock_type: - raise ValueError("until's clock type does not match this clock's type") - - event = _rclpy.ClockEvent() - time_source_changed = False - - def on_time_jump(time_jump: TimeJump): - """Wake when time jumps and is past target time.""" - nonlocal time_source_changed - - # ROS time being activated or deactivated changes the epoch, so sleep - # time loses its meaning - time_source_changed = ( - time_source_changed or - ClockChange.ROS_TIME_ACTIVATED == time_jump.clock_change or - ClockChange.ROS_TIME_DEACTIVATED == time_jump.clock_change) - - if time_source_changed or self.now() >= until: - event.set() - - # Wake when context is shut down - context.on_shutdown(event.set) - - threshold = JumpThreshold( - min_forward=Duration(nanoseconds=1), - min_backward=None, - on_clock_change=True) - with self.create_jump_callback(threshold, post_callback=on_time_jump): - if ClockType.SYSTEM_TIME == self._clock_type: - event.wait_until_system(self.__clock, until._time_handle) - elif ClockType.STEADY_TIME == self._clock_type: - event.wait_until_steady(self.__clock, until._time_handle) - elif ClockType.ROS_TIME == self._clock_type: - event.wait_until_ros(self.__clock, until._time_handle) - - if not context.ok() or time_source_changed: - return False - - return self.now() >= until - - def sleep_for(self, rel_time: Duration, context=None) -> bool: - """ - Sleep for a specified duration. - - Equivalent to: - - .. code-block:: python - - clock.sleep_until(clock.now() + rel_time, context) - - - When using a ROSClock, this may sleep forever if the TimeSource is misconfigured and the - context is never shut down. - ROS time being activated or deactivated causes this function to cease sleeping and return - False. - - :param rel_time: Duration of time to sleep for. - :param context: Context which when shut down will cause this sleep to wake early. - If context is None, then the default context is used. - :return: True if the full duration was slept, or False if it woke for another reason. - :raises NotInitializedException: context has not been initialized or is shutdown. - """ - return self.sleep_until(self.now() + rel_time, context) - - -class ROSClock(Clock): - - def __new__(cls): - return super().__new__(Clock, clock_type=ClockType.ROS_TIME) - - @property - def ros_time_is_active(self): - with self.handle: - return self.handle.get_ros_time_override_is_enabled() - - 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: - self.handle.set_ros_time_override_is_enabled(enabled) - - def set_ros_time_override(self, time: Time): - if not isinstance(time, Time): - raise TypeError( - 'Time must be specified as rclpy.time.Time. Received type: {0}'.format(type(time))) - with self.handle: - self.handle.set_ros_time_override(time._time_handle) diff --git a/src/rclpy/constants.py b/src/rclpy/constants.py deleted file mode 100644 index c01dd5c..0000000 --- a/src/rclpy/constants.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -S_TO_NS = 1000 * 1000 * 1000 diff --git a/src/rclpy/context.py b/src/rclpy/context.py deleted file mode 100644 index 4e86d69..0000000 --- a/src/rclpy/context.py +++ /dev/null @@ -1,151 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from inspect import ismethod -import sys -import threading -from typing import Callable -from typing import List -from typing import Optional -import weakref - - -g_logging_configure_lock = threading.Lock() -g_logging_ref_count = 0 - - -class Context: - """ - Encapsulates the lifecycle of init and shutdown. - - Context objects should not be reused, and are finalized in their destructor. - Wraps the `rcl_context_t` type. - """ - - def __init__(self): - self._lock = threading.Lock() - self._callbacks = [] - self._logging_initialized = False - self.__context = None - - @property - def handle(self): - return self.__context - - def destroy(self): - self.__context.destroy_when_not_in_use() - - def init(self, - args: Optional[List[str]] = None, - *, - initialize_logging: bool = True, - domain_id: Optional[int] = None): - """ - Initialize ROS communications for a given context. - - :param args: List of command line arguments. - """ - # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - global g_logging_ref_count - with self._lock: - if domain_id is not None and domain_id < 0: - raise RuntimeError( - 'Domain id ({}) should not be lower than zero.' - .format(domain_id)) - - if self.__context is not None: - raise RuntimeError('Context.init() must only be called once') - - self.__context = _rclpy.Context( - args if args is not None else sys.argv, - domain_id if domain_id is not None else _rclpy.RCL_DEFAULT_DOMAIN_ID) - if initialize_logging and not self._logging_initialized: - with g_logging_configure_lock: - g_logging_ref_count += 1 - if g_logging_ref_count == 1: - _rclpy.rclpy_logging_configure(self.__context) - self._logging_initialized = True - - def ok(self): - """Check if context hasn't been shut down.""" - with self._lock: - if self.__context is None: - return False - with self.__context: - return self.__context.ok() - - def _call_on_shutdown_callbacks(self): - for weak_method in self._callbacks: - callback = weak_method() - if callback is not None: - callback() - self._callbacks = [] - - def shutdown(self): - """Shutdown this context.""" - if self.__context is None: - raise RuntimeError('Context must be initialized before it can be shutdown') - with self.__context, self._lock: - self.__context.shutdown() - self._call_on_shutdown_callbacks() - self._logging_fini() - - def try_shutdown(self): - """Shutdown this context, if not already shutdown.""" - if self.__context is None: - return - with self.__context, self._lock: - if self.__context.ok(): - self.__context.shutdown() - self._call_on_shutdown_callbacks() - self._logging_fini() - - def _remove_callback(self, weak_method): - self._callbacks.remove(weak_method) - - def on_shutdown(self, callback: Callable[[], None]): - """Add a callback to be called on shutdown.""" - if not callable(callback): - raise TypeError('callback should be a callable, got {}', type(callback)) - with self.__context, self._lock: - if not self.__context.ok(): - callback() - else: - if ismethod(callback): - self._callbacks.append(weakref.WeakMethod(callback, self._remove_callback)) - else: - self._callbacks.append(callback) - - def _logging_fini(self): - # This function must be called with self._lock held. - from rclpy.impl.implementation_singleton import rclpy_implementation - global g_logging_ref_count - if self._logging_initialized: - with g_logging_configure_lock: - g_logging_ref_count -= 1 - if g_logging_ref_count == 0: - rclpy_implementation.rclpy_logging_fini() - if g_logging_ref_count < 0: - raise RuntimeError( - 'Unexpected error: logger ref count should never be lower that zero') - self._logging_initialized = False - - def get_domain_id(self): - """Get domain id of context.""" - if self.__context is None: - raise RuntimeError('Context must be initialized before it can have a domain id') - with self.__context, self._lock: - return self.__context.get_domain_id() diff --git a/src/rclpy/duration.py b/src/rclpy/duration.py deleted file mode 100644 index 3e54409..0000000 --- a/src/rclpy/duration.py +++ /dev/null @@ -1,108 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import builtin_interfaces.msg -from rclpy.constants import S_TO_NS -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -class Duration: - """A period between two time points, with nanosecond precision.""" - - def __init__(self, *, seconds=0, nanoseconds=0): - """ - Create an instance of :class:`Duration`, combined from given seconds and nanoseconds. - - :param seconds: Time span seconds, if any, fractional part will be included. - :param nanoseconds: Time span nanoseconds, if any, fractional part will be discarded. - """ - total_nanoseconds = int(seconds * S_TO_NS) - total_nanoseconds += int(nanoseconds) - if total_nanoseconds >= 2**63 or total_nanoseconds < -2**63: - # pybind11 would raise TypeError, but we want OverflowError - raise OverflowError( - 'Total nanoseconds value is too large to store in C duration.') - self._duration_handle = _rclpy.rcl_duration_t(total_nanoseconds) - - @property - def nanoseconds(self): - return self._duration_handle.nanoseconds - - def __repr__(self): - return 'Duration(nanoseconds={0})'.format(self.nanoseconds) - - def __str__(self): - if self == Infinite: - return 'Infinite' - return f'{self.nanoseconds} nanoseconds' - - def __eq__(self, other): - if isinstance(other, Duration): - return self.nanoseconds == other.nanoseconds - # Raise instead of returning NotImplemented to prevent comparison with invalid types, - # e.g. ints. - # Otherwise `Duration(nanoseconds=5) == 5` will return False instead of raising, and this - # could lead to hard-to-find bugs. - raise TypeError("Can't compare duration with object of type: ", type(other)) - - def __ne__(self, other): - return not self.__eq__(other) - - def __lt__(self, other): - if isinstance(other, Duration): - return self.nanoseconds < other.nanoseconds - return NotImplemented - - def __le__(self, other): - if isinstance(other, Duration): - return self.nanoseconds <= other.nanoseconds - return NotImplemented - - def __gt__(self, other): - if isinstance(other, Duration): - return self.nanoseconds > other.nanoseconds - return NotImplemented - - def __ge__(self, other): - if isinstance(other, Duration): - return self.nanoseconds >= other.nanoseconds - return NotImplemented - - def to_msg(self): - """ - Get duration as :class:`builtin_interfaces.msg.Duration`. - - :returns: duration as message - :rtype: builtin_interfaces.msg.Duration - """ - seconds, nanoseconds = divmod(self.nanoseconds, S_TO_NS) - return builtin_interfaces.msg.Duration(sec=seconds, nanosec=nanoseconds) - - @classmethod - def from_msg(cls, msg): - """ - Create an instance of :class:`Duration` from a duration message. - - :param msg: An instance of :class:`builtin_interfaces.msg.Duration`. - """ - if not isinstance(msg, builtin_interfaces.msg.Duration): - raise TypeError('Must pass a builtin_interfaces.msg.Duration object') - return cls(seconds=msg.sec, nanoseconds=msg.nanosec) - - def get_c_duration(self): - return self._duration_handle - - -# Constant representing an infinite amount of time. -Infinite = Duration(nanoseconds=_rclpy.RMW_DURATION_INFINITE) diff --git a/src/rclpy/exceptions.py b/src/rclpy/exceptions.py deleted file mode 100644 index 4dc1e88..0000000 --- a/src/rclpy/exceptions.py +++ /dev/null @@ -1,149 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -class NotInitializedException(Exception): - """Raised when the rclpy implementation is accessed before rclpy.init().""" - - def __init__(self, *args): - Exception.__init__(self, 'rclpy.init() has not been called', *args) - - -class NoTypeSupportImportedException(Exception): - """Raised when there is no type support imported.""" - - def __init__(self, *args): - Exception.__init__(self, 'no type_support imported') - - -class NameValidationException(Exception): - """Raised when a topic name, node name, or namespace are invalid.""" - - def __init__(self, name_type, name, error_msg, invalid_index, *args): - msg = """\ -Invalid {name_type}: {error_msg}: - '{name}' - {indent}^\ -""".format(name_type=name_type, name=name, error_msg=error_msg, indent=' ' * invalid_index) - Exception.__init__(self, msg) - - -InvalidHandle = _rclpy.InvalidHandle - - -class InvalidNamespaceException(NameValidationException): - """Raised when a namespace is invalid.""" - - def __init__(self, name, error_msg, invalid_index, *args): - NameValidationException.__init__(self, 'namespace', name, error_msg, invalid_index) - - -class InvalidNodeNameException(NameValidationException): - """Raised when a node name is invalid.""" - - def __init__(self, name, error_msg, invalid_index, *args): - NameValidationException.__init__(self, 'node name', name, error_msg, invalid_index) - - -class InvalidTopicNameException(NameValidationException): - """Raised when a topic name is invalid.""" - - def __init__(self, name, error_msg, invalid_index, *args): - NameValidationException.__init__(self, 'topic name', name, error_msg, invalid_index) - - -class InvalidServiceNameException(NameValidationException): - """Raised when a service name is invalid.""" - - def __init__(self, name, error_msg, invalid_index, *args): - NameValidationException.__init__(self, 'service name', name, error_msg, invalid_index) - - -class ParameterException(Exception): - """Base exception for parameter-related errors.""" - - def __init__(self, error_msg, parameters, *args): - Exception.__init__(self, f'{error_msg}: {parameters}') - - -class ParameterNotDeclaredException(ParameterException): - """Raised when handling an undeclared parameter when it is not allowed.""" - - def __init__(self, parameters, *args): - Exception.__init__(self, 'Invalid access to undeclared parameter(s)', parameters, *args) - - -class ParameterAlreadyDeclaredException(ParameterException): - """Raised when declaring a parameter that had been declared before.""" - - def __init__(self, parameters, *args): - Exception.__init__(self, 'Parameter(s) already declared', parameters, *args) - - -class InvalidParameterException(ParameterException): - """Raised when a parameter to be declared has an invalid name.""" - - def __init__(self, parameter, *args): - Exception.__init__(self, 'Invalid parameter name', parameter, *args) - - -class InvalidParameterTypeException(ParameterException): - """Raised when a parameter is rejected for having an invalid type.""" - - def __init__(self, desired_parameter, expected_type, *args): - from rclpy.parameter import Parameter - Exception.__init__( - self, - f"Trying to set parameter '{desired_parameter._name}' to '{desired_parameter._value}'" - f" of type '{Parameter.Type.from_parameter_value(desired_parameter._value).name}'" - f", expecting type '{expected_type}'", - *args) - self._actual_type = desired_parameter.type_ - self._param_name = desired_parameter.name - - -class InvalidParameterValueException(ParameterException): - """Raised when a parameter is rejected by a user callback or when applying a descriptor.""" - - def __init__(self, parameter, value, reason, *args): - Exception.__init__( - self, - 'Invalid parameter value ({value}) for parameter. Reason: {reason}'.format( - value=value, reason=reason), parameter, *args) - - -class ParameterImmutableException(ParameterException): - """Raised when a read-only parameter is modified.""" - - def __init__(self, parameter, *args): - Exception.__init__(self, 'Attempted to modify read-only parameter', parameter, *args) - - -class ParameterUninitializedException(ParameterException): - """Raised when an uninitialized parameter is accessed.""" - - def __init__(self, parameter_name, *args): - Exception.__init__( - self, - f"The parameter '{parameter_name}' is not initialized", - *args) - - -class ROSInterruptException(Exception): - """Raised when an operation is canceled by rclpy shutting down.""" - - def __init__(self, *args): - Exception.__init__(self, 'rclpy.shutdown() has been called', *args) diff --git a/src/rclpy/executors.py b/src/rclpy/executors.py deleted file mode 100644 index 7579bf8..0000000 --- a/src/rclpy/executors.py +++ /dev/null @@ -1,801 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from concurrent.futures import ThreadPoolExecutor -from contextlib import ExitStack -import inspect -import multiprocessing -from threading import Condition -from threading import Lock -from threading import RLock -import time -from typing import Any -from typing import Callable -from typing import Coroutine -from typing import Generator -from typing import List -from typing import Optional -from typing import Set -from typing import Tuple -from typing import TYPE_CHECKING -from typing import TypeVar -from typing import Union - - -from rclpy.client import Client -from rclpy.clock import Clock -from rclpy.clock import ClockType -from rclpy.context import Context -from rclpy.exceptions import InvalidHandle -from rclpy.guard_condition import GuardCondition -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.service import Service -from rclpy.signals import SignalHandlerGuardCondition -from rclpy.subscription import Subscription -from rclpy.task import Future -from rclpy.task import Task -from rclpy.timer import Timer -from rclpy.utilities import get_default_context -from rclpy.utilities import timeout_sec_to_nsec -from rclpy.waitable import NumberOfEntities -from rclpy.waitable import Waitable - -# For documentation purposes -# TODO(jacobperron): Make all entities implement the 'Waitable' interface for better type checking -WaitableEntityType = TypeVar('WaitableEntityType') - -# Avoid import cycle -if TYPE_CHECKING: - from rclpy.node import Node # noqa: F401 - - -class _WorkTracker: - """Track the amount of work that is in progress.""" - - def __init__(self): - # Number of tasks that are being executed - self._num_work_executing = 0 - self._work_condition = Condition() - - def __enter__(self): - """Increment the amount of executing work by 1.""" - with self._work_condition: - self._num_work_executing += 1 - - def __exit__(self, t, v, tb): - """Decrement the amount of work executing by 1.""" - with self._work_condition: - self._num_work_executing -= 1 - self._work_condition.notify_all() - - def wait(self, timeout_sec=None): - """ - Wait until all work completes. - - :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0 - :type timeout_sec: float or None - :rtype: bool True if all work completed - """ - if timeout_sec is not None and timeout_sec < 0: - timeout_sec = None - # Wait for all work to complete - with self._work_condition: - if not self._work_condition.wait_for( - lambda: self._num_work_executing == 0, timeout_sec): - return False - return True - - -async def await_or_execute(callback: Union[Callable, Coroutine], *args) -> Any: - """Await a callback if it is a coroutine, else execute it.""" - if inspect.iscoroutinefunction(callback): - # Await a coroutine - return await callback(*args) - else: - # Call a normal function - return callback(*args) - - -class TimeoutException(Exception): - """Signal that a timeout occurred.""" - - pass - - -class ShutdownException(Exception): - """Signal that executor was shut down.""" - - pass - - -class ExternalShutdownException(Exception): - """Context has been shutdown.""" - - pass - - -class ConditionReachedException(Exception): - """Future has been completed.""" - - pass - - -class TimeoutObject: - """Use timeout object to save timeout.""" - - def __init__(self, timeout: float): - self._timeout = timeout - - @property - def timeout(self): - return self._timeout - - @timeout.setter - def timeout(self, timeout): - self._timeout = timeout - - -class Executor: - """ - The base class for an executor. - - An executor controls the threading model used to process callbacks. Callbacks are units of work - like subscription callbacks, timer callbacks, service calls, and received client responses. An - executor controls which threads callbacks get executed in. - - A custom executor must define :meth:`spin_once`. - If the executor has any cleanup then it should also define :meth:`shutdown`. - - :param context: The context to be associated with, or ``None`` for the default global context. - """ - - def __init__(self, *, context: Context = None) -> None: - super().__init__() - self._context = get_default_context() if context is None else context - self._nodes: Set[Node] = set() - self._nodes_lock = RLock() - # Tasks to be executed (oldest first) 3-tuple Task, Entity, Node - self._tasks: List[Tuple[Task, Optional[WaitableEntityType], Optional[Node]]] = [] - self._tasks_lock = Lock() - # This is triggered when wait_for_ready_callbacks should rebuild the wait list - self._guard = GuardCondition( - callback=None, callback_group=None, context=self._context) - # True if shutdown has been called - self._is_shutdown = False - self._work_tracker = _WorkTracker() - # Protect against shutdown() being called in parallel in two threads - self._shutdown_lock = Lock() - # State for wait_for_ready_callbacks to reuse generator - self._cb_iter = None - self._last_args = None - self._last_kwargs = None - # Executor cannot use ROS clock because that requires a node - self._clock = Clock(clock_type=ClockType.STEADY_TIME) - self._sigint_gc = SignalHandlerGuardCondition(context) - self._context.on_shutdown(self.wake) - - @property - def context(self) -> Context: - """Get the context associated with the executor.""" - return self._context - - def create_task(self, callback: Union[Callable, Coroutine], *args, **kwargs) -> Task: - """ - Add a callback or coroutine to be executed during :meth:`spin` and return a Future. - - Arguments to this function are passed to the callback. - - :param callback: A callback to be run in the executor. - """ - task = Task(callback, args, kwargs, executor=self) - with self._tasks_lock: - self._tasks.append((task, None, None)) - self._guard.trigger() - # Task inherits from Future - return task - - def shutdown(self, timeout_sec: float = None) -> bool: - """ - Stop executing callbacks and wait for their completion. - - :param timeout_sec: Seconds to wait. Block forever if ``None`` or negative. - Don't wait if 0. - :return: ``True`` if all outstanding callbacks finished executing, or ``False`` if the - timeot expires before all outstanding work is done. - """ - with self._shutdown_lock: - if not self._is_shutdown: - self._is_shutdown = True - # Tell executor it's been shut down - self._guard.trigger() - - if not self._work_tracker.wait(timeout_sec): - return False - - # Clean up stuff that won't be used anymore - with self._nodes_lock: - self._nodes = set() - - with self._shutdown_lock: - if self._guard: - self._guard.destroy() - self._guard = None - if self._sigint_gc: - self._sigint_gc.destroy() - self._sigint_gc = None - self._cb_iter = None - self._last_args = None - self._last_kwargs = None - return True - - def __del__(self): - if self._sigint_gc is not None: - self._sigint_gc.destroy() - - def add_node(self, node: 'Node') -> bool: - """ - Add a node whose callbacks should be managed by this executor. - - :param node: The node to add to the executor. - :return: ``True`` if the node was added, ``False`` otherwise. - """ - with self._nodes_lock: - if node not in self._nodes: - self._nodes.add(node) - node.executor = self - # Rebuild the wait set so it includes this new node - self._guard.trigger() - return True - return False - - def remove_node(self, node: 'Node') -> None: - """ - Stop managing this node's callbacks. - - :param node: The node to remove from the executor. - """ - with self._nodes_lock: - try: - self._nodes.remove(node) - except KeyError: - pass - else: - # Rebuild the wait set so it doesn't include this node - self._guard.trigger() - - def wake(self) -> None: - """ - Wake the executor because something changed. - - This is used to tell the executor when entities are created or destroyed. - """ - if self._guard: - self._guard.trigger() - - def get_nodes(self) -> List['Node']: - """Return nodes that have been added to this executor.""" - with self._nodes_lock: - return list(self._nodes) - - def spin(self) -> None: - """Execute callbacks until shutdown.""" - while self._context.ok() and not self._is_shutdown: - self.spin_once() - - def spin_until_future_complete(self, future: Future, timeout_sec: float = None) -> None: - """Execute callbacks until a given future is done or a timeout occurs.""" - # Make sure the future wakes this executor when it is done - future.add_done_callback(lambda x: self.wake()) - - if timeout_sec is None or timeout_sec < 0: - while self._context.ok() and not future.done() and not self._is_shutdown: - self.spin_once_until_future_complete(future, timeout_sec) - else: - start = time.monotonic() - end = start + timeout_sec - timeout_left = TimeoutObject(timeout_sec) - - while self._context.ok() and not future.done() and not self._is_shutdown: - self.spin_once_until_future_complete(future, timeout_left) - now = time.monotonic() - - if now >= end: - return - - timeout_left.timeout = end - now - - def spin_once(self, timeout_sec: float = None) -> None: - """ - Wait for and execute a single callback. - - A custom executor should use :meth:`wait_for_ready_callbacks` to get work. - - :param timeout_sec: Seconds to wait. Block forever if ``None`` or negative. - Don't wait if 0. - """ - raise NotImplementedError() - - def spin_once_until_future_complete( - self, - future: Future, - timeout_sec: Optional[Union[float, TimeoutObject]] = None - ) -> None: - """ - Wait for and execute a single callback. - - This should behave in the same way as :meth:`spin_once`. - If needed by the implementation, it should awake other threads waiting. - - :param future: The executor will wait until this future is done. - :param timeout_sec: Maximum seconds to wait. Block forever if ``None`` or negative. - Don't wait if 0. - """ - raise NotImplementedError() - - def _take_timer(self, tmr): - with tmr.handle: - tmr.handle.call_timer() - - async def _execute_timer(self, tmr, _): - await await_or_execute(tmr.callback) - - def _take_subscription(self, sub): - with sub.handle: - msg_info = sub.handle.take_message(sub.msg_type, sub.raw) - if msg_info is not None: - return msg_info[0] - return None - - async def _execute_subscription(self, sub, msg): - if msg: - await await_or_execute(sub.callback, msg) - - def _take_client(self, client): - with client.handle: - return client.handle.take_response(client.srv_type.Response) - - async def _execute_client(self, client, seq_and_response): - header, response = seq_and_response - if header is not None: - try: - sequence = header.request_id.sequence_number - future = client.get_pending_request(sequence) - except KeyError: - # The request was cancelled - pass - else: - future._set_executor(self) - future.set_result(response) - - def _take_service(self, srv): - with srv.handle: - request_and_header = srv.handle.service_take_request(srv.srv_type.Request) - return request_and_header - - async def _execute_service(self, srv, request_and_header): - if request_and_header is None: - return - (request, header) = request_and_header - if request: - response = await await_or_execute(srv.callback, request, srv.srv_type.Response()) - srv.send_response(response, header) - - def _take_guard_condition(self, gc): - gc._executor_triggered = False - - async def _execute_guard_condition(self, gc, _): - await await_or_execute(gc.callback) - - async def _execute_waitable(self, waitable, data): - for future in waitable._futures: - future._set_executor(self) - await waitable.execute(data) - - def _make_handler( - self, - entity: WaitableEntityType, - node: 'Node', - take_from_wait_list: Callable, - call_coroutine: Coroutine - ) -> Task: - """ - Make a handler that performs work on an entity. - - :param entity: An entity to wait on. - :param node: The node associated with the entity. - :param take_from_wait_list: Makes the entity to stop appearing in the wait list. - :param call_coroutine: Does the work the entity is ready for - """ - # Mark this so it doesn't get added back to the wait list - entity._executor_event = True - - async def handler(entity, gc, is_shutdown, work_tracker): - if is_shutdown or not entity.callback_group.beginning_execution(entity): - # Didn't get the callback, or the executor has been ordered to stop - entity._executor_event = False - gc.trigger() - return - with work_tracker: - arg = take_from_wait_list(entity) - - # Signal that this has been 'taken' and can be added back to the wait list - entity._executor_event = False - gc.trigger() - - try: - await call_coroutine(entity, arg) - finally: - entity.callback_group.ending_execution(entity) - # Signal that work has been done so the next callback in a mutually exclusive - # callback group can get executed - gc.trigger() - task = Task( - handler, (entity, self._guard, self._is_shutdown, self._work_tracker), - executor=self) - with self._tasks_lock: - self._tasks.append((task, entity, node)) - return task - - def can_execute(self, entity: WaitableEntityType) -> bool: - """ - Determine if a callback for an entity can be executed. - - :param entity: Subscription, Timer, Guard condition, etc - :returns: ``True`` if the entity callback can be executed, ``False`` otherwise. - """ - return not entity._executor_event and entity.callback_group.can_execute(entity) - - def _wait_for_ready_callbacks( - self, - timeout_sec: Optional[Union[float, TimeoutObject]] = None, - nodes: List['Node'] = None, - condition: Callable[[], bool] = lambda: False, - ) -> Generator[Tuple[Task, WaitableEntityType, 'Node'], None, None]: - """ - Yield callbacks that are ready to be executed. - - :raise TimeoutException: on timeout. - :raise ShutdownException: on if executor was shut down. - - :param timeout_sec: Seconds to wait. Block forever if ``None`` or negative. - Don't wait if 0. - :param nodes: A list of nodes to wait on. Wait on all nodes if ``None``. - :param condition: A callable that makes the function return immediately when it evaluates - to True. - """ - timeout_timer = None - timeout_nsec = timeout_sec_to_nsec( - timeout_sec.timeout if isinstance(timeout_sec, TimeoutObject) else timeout_sec) - if timeout_nsec > 0: - timeout_timer = Timer(None, None, timeout_nsec, self._clock, context=self._context) - - yielded_work = False - while not yielded_work and not self._is_shutdown and not condition(): - # Refresh "all" nodes in case executor was woken by a node being added or removed - nodes_to_use = nodes - if nodes is None: - nodes_to_use = self.get_nodes() - - # Yield tasks in-progress before waiting for new work - tasks = None - with self._tasks_lock: - tasks = list(self._tasks) - if tasks: - for task, entity, node in reversed(tasks): - if (not task.executing() and not task.done() and - (node is None or node in nodes_to_use)): - yielded_work = True - yield task, entity, node - with self._tasks_lock: - # Get rid of any tasks that are done - self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks)) - - # Gather entities that can be waited on - subscriptions: List[Subscription] = [] - guards: List[GuardCondition] = [] - timers: List[Timer] = [] - clients: List[Client] = [] - services: List[Service] = [] - waitables: List[Waitable] = [] - for node in nodes_to_use: - subscriptions.extend(filter(self.can_execute, node.subscriptions)) - timers.extend(filter(self.can_execute, node.timers)) - clients.extend(filter(self.can_execute, node.clients)) - services.extend(filter(self.can_execute, node.services)) - node_guards = filter(self.can_execute, node.guards) - waitables.extend(filter(self.can_execute, node.waitables)) - # retrigger a guard condition that was triggered but not handled - for gc in node_guards: - if gc._executor_triggered: - gc.trigger() - guards.append(gc) - if timeout_timer is not None: - timers.append(timeout_timer) - - guards.append(self._guard) - guards.append(self._sigint_gc) - - entity_count = NumberOfEntities( - len(subscriptions), len(guards), len(timers), len(clients), len(services)) - - # Construct a wait set - wait_set = None - with ExitStack() as context_stack: - sub_handles = [] - for sub in subscriptions: - try: - context_stack.enter_context(sub.handle) - sub_handles.append(sub.handle) - except InvalidHandle: - entity_count.num_subscriptions -= 1 - - client_handles = [] - for cli in clients: - try: - context_stack.enter_context(cli.handle) - client_handles.append(cli.handle) - except InvalidHandle: - entity_count.num_clients -= 1 - - service_handles = [] - for srv in services: - try: - context_stack.enter_context(srv.handle) - service_handles.append(srv.handle) - except InvalidHandle: - entity_count.num_services -= 1 - - timer_handles = [] - for tmr in timers: - try: - context_stack.enter_context(tmr.handle) - timer_handles.append(tmr.handle) - except InvalidHandle: - entity_count.num_timers -= 1 - - guard_handles = [] - for gc in guards: - try: - context_stack.enter_context(gc.handle) - guard_handles.append(gc.handle) - except InvalidHandle: - entity_count.num_guard_conditions -= 1 - - for waitable in waitables: - try: - context_stack.enter_context(waitable) - entity_count += waitable.get_num_entities() - except InvalidHandle: - pass - - context_stack.enter_context(self._context.handle) - - wait_set = _rclpy.WaitSet( - entity_count.num_subscriptions, - entity_count.num_guard_conditions, - entity_count.num_timers, - entity_count.num_clients, - entity_count.num_services, - entity_count.num_events, - self._context.handle) - - wait_set.clear_entities() - for sub_handle in sub_handles: - wait_set.add_subscription(sub_handle) - for cli_handle in client_handles: - wait_set.add_client(cli_handle) - for srv_capsule in service_handles: - wait_set.add_service(srv_capsule) - for tmr_handle in timer_handles: - wait_set.add_timer(tmr_handle) - for gc_handle in guard_handles: - wait_set.add_guard_condition(gc_handle) - for waitable in waitables: - waitable.add_to_wait_set(wait_set) - - # Wait for something to become ready - wait_set.wait(timeout_nsec) - if self._is_shutdown: - raise ShutdownException() - if not self._context.ok(): - raise ExternalShutdownException() - - # get ready entities - subs_ready = wait_set.get_ready_entities('subscription') - guards_ready = wait_set.get_ready_entities('guard_condition') - timers_ready = wait_set.get_ready_entities('timer') - clients_ready = wait_set.get_ready_entities('client') - services_ready = wait_set.get_ready_entities('service') - - # Mark all guards as triggered before yielding since they're auto-taken - for gc in guards: - if gc.handle.pointer in guards_ready: - gc._executor_triggered = True - - # Check waitables before wait set is destroyed - for node in nodes_to_use: - for wt in node.waitables: - # Only check waitables that were added to the wait set - if wt in waitables and wt.is_ready(wait_set): - if wt.callback_group.can_execute(wt): - handler = self._make_handler( - wt, node, lambda e: e.take_data(), self._execute_waitable) - yielded_work = True - yield handler, wt, node - - # Process ready entities one node at a time - for node in nodes_to_use: - for tmr in node.timers: - if tmr.handle.pointer in timers_ready: - # Check timer is ready to workaround rcl issue with cancelled timers - if tmr.handle.is_timer_ready(): - if tmr.callback_group.can_execute(tmr): - handler = self._make_handler( - tmr, node, self._take_timer, self._execute_timer) - yielded_work = True - yield handler, tmr, node - - for sub in node.subscriptions: - if sub.handle.pointer in subs_ready: - if sub.callback_group.can_execute(sub): - handler = self._make_handler( - sub, node, self._take_subscription, self._execute_subscription) - yielded_work = True - yield handler, sub, node - - for gc in node.guards: - if gc._executor_triggered: - if gc.callback_group.can_execute(gc): - handler = self._make_handler( - gc, node, self._take_guard_condition, - self._execute_guard_condition) - yielded_work = True - yield handler, gc, node - - for client in node.clients: - if client.handle.pointer in clients_ready: - if client.callback_group.can_execute(client): - handler = self._make_handler( - client, node, self._take_client, self._execute_client) - yielded_work = True - yield handler, client, node - - for srv in node.services: - if srv.handle.pointer in services_ready: - if srv.callback_group.can_execute(srv): - handler = self._make_handler( - srv, node, self._take_service, self._execute_service) - yielded_work = True - yield handler, srv, node - - # Check timeout timer - if ( - timeout_nsec == 0 or - (timeout_timer is not None and timeout_timer.handle.pointer in timers_ready) - ): - raise TimeoutException() - if self._is_shutdown: - raise ShutdownException() - if condition(): - raise ConditionReachedException() - - def wait_for_ready_callbacks(self, *args, **kwargs) -> Tuple[Task, WaitableEntityType, 'Node']: - """ - Return callbacks that are ready to be executed. - - The arguments to this function are passed to the internal method - :meth:`_wait_for_ready_callbacks` to get a generator for ready callbacks: - - .. Including the docstring for the hidden function for reference - .. automethod:: _wait_for_ready_callbacks - """ - while True: - if self._cb_iter is None or self._last_args != args or self._last_kwargs != kwargs: - # Create a new generator - self._last_args = args - self._last_kwargs = kwargs - self._cb_iter = self._wait_for_ready_callbacks(*args, **kwargs) - - try: - return next(self._cb_iter) - except StopIteration: - # Generator ran out of work - self._cb_iter = None - - -class SingleThreadedExecutor(Executor): - """Runs callbacks in the thread that calls :meth:`Executor.spin`.""" - - def __init__(self, *, context: Context = None) -> None: - super().__init__(context=context) - - def _spin_once_impl( - self, - timeout_sec: Optional[Union[float, TimeoutObject]] = None - ) -> None: - try: - handler, entity, node = self.wait_for_ready_callbacks(timeout_sec=timeout_sec) - except ShutdownException: - pass - except TimeoutException: - pass - else: - handler() - if handler.exception() is not None: - raise handler.exception() - - def spin_once(self, timeout_sec: float = None) -> None: - self._spin_once_impl(timeout_sec) - - def spin_once_until_future_complete( - self, - future: Future, - timeout_sec: Optional[Union[float, TimeoutObject]] = None - ) -> None: - self._spin_once_impl(timeout_sec) - - -class MultiThreadedExecutor(Executor): - """ - Runs callbacks in a pool of threads. - - :param num_threads: number of worker threads in the pool. If ``None``, the number of threads - will use :func:`multiprocessing.cpu_count`. If that's not implemented the number of threads - defaults to 1. - :param context: The context associated with the executor. - """ - - def __init__(self, num_threads: int = None, *, context: Context = None) -> None: - super().__init__(context=context) - if num_threads is None: - try: - num_threads = multiprocessing.cpu_count() - except NotImplementedError: - num_threads = 1 - self._futures = [] - self._executor = ThreadPoolExecutor(num_threads) - - def _spin_once_impl( - self, - timeout_sec: Optional[Union[float, TimeoutObject]] = None, - wait_condition: Callable[[], bool] = lambda: False - ) -> None: - try: - handler, entity, node = self.wait_for_ready_callbacks( - timeout_sec, None, wait_condition) - except ExternalShutdownException: - pass - except ShutdownException: - pass - except TimeoutException: - pass - except ConditionReachedException: - pass - else: - self._executor.submit(handler) - self._futures.append(handler) - for future in self._futures: # check for any exceptions - if future.done(): - self._futures.remove(future) - future.result() - - def spin_once(self, timeout_sec: float = None) -> None: - self._spin_once_impl(timeout_sec) - - def spin_once_until_future_complete( - self, - future: Future, - timeout_sec: Optional[Union[float, TimeoutObject]] = None - ) -> None: - self._spin_once_impl(timeout_sec, future.done) diff --git a/src/rclpy/expand_topic_name.py b/src/rclpy/expand_topic_name.py deleted file mode 100644 index 8d8130b..0000000 --- a/src/rclpy/expand_topic_name.py +++ /dev/null @@ -1,33 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -def expand_topic_name(topic_name, node_name, node_namespace): - """ - Expand a given topic name using given node name and namespace as well. - - Note that this function can succeed but the expanded topic name may still - be invalid. - The :py:func:validate_full_topic_name(): should be used on the expanded - topic name to ensure it is valid after expansion. - - :param topic_name str: topic name to be expanded - :param node_name str: name of the node that this topic is associated with - :param namespace str: namespace that the topic is within - :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) diff --git a/src/rclpy/guard_condition.py b/src/rclpy/guard_condition.py deleted file mode 100644 index 8b83df6..0000000 --- a/src/rclpy/guard_condition.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.utilities import get_default_context - - -class GuardCondition: - - def __init__(self, callback, callback_group, context=None): - self._context = get_default_context() if context is None else context - with self._context.handle: - self.__gc = _rclpy.GuardCondition(self._context.handle) - self.callback = callback - self.callback_group = callback_group - # True when the callback is ready to fire but has not been "taken" by an executor - self._executor_event = False - # True when the executor sees this has been triggered but has not yet been handled - self._executor_triggered = False - - def trigger(self): - with self.__gc: - self.__gc.trigger_guard_condition() - - @property - def handle(self): - return self.__gc - - def destroy(self): - self.handle.destroy_when_not_in_use() diff --git a/src/rclpy/impl/__init__.py b/src/rclpy/impl/__init__.py deleted file mode 100644 index 95787e1..0000000 --- a/src/rclpy/impl/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright 2016-2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. diff --git a/src/rclpy/impl/implementation_singleton.py b/src/rclpy/impl/implementation_singleton.py deleted file mode 100644 index ba7f55f..0000000 --- a/src/rclpy/impl/implementation_singleton.py +++ /dev/null @@ -1,32 +0,0 @@ -# Copyright 2016-2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -Provide singleton access to the rclpy C modules. - -For example, you might use it like this: - -.. code:: - - from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - _rclpy.rclpy_init() - while _rclpy.rclpy_ok(): - # ... -""" - -from rpyutils import import_c_library -package = 'rclpy' - -rclpy_implementation = import_c_library('._rclpy_pybind11', package) diff --git a/src/rclpy/impl/logging_severity.py b/src/rclpy/impl/logging_severity.py deleted file mode 100644 index 6392cc7..0000000 --- a/src/rclpy/impl/logging_severity.py +++ /dev/null @@ -1,33 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from enum import IntEnum - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -class LoggingSeverity(IntEnum): - """ - Enum for logging severity levels. - - This enum must match the one defined in rcutils/logging.h - """ - - UNSET = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_UNSET - DEBUG = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_DEBUG - INFO = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_INFO - WARN = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_WARN - ERROR = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_ERROR - FATAL = _rclpy.RCUTILS_LOG_SEVERITY.RCUTILS_LOG_SEVERITY_FATAL diff --git a/src/rclpy/impl/rcutils_logger.py b/src/rclpy/impl/rcutils_logger.py deleted file mode 100644 index f94a44d..0000000 --- a/src/rclpy/impl/rcutils_logger.py +++ /dev/null @@ -1,349 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from collections import namedtuple -from collections import OrderedDict -import inspect -import os - -from rclpy.clock import Clock -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.impl.logging_severity import LoggingSeverity - -# Known filenames from which logging methods can be called (will be ignored in `_find_caller`). -_internal_callers = [] -# This will cause rclpy filenames to be registered in `_internal_callers` on first logging call. -_populate_internal_callers = True - - -def _find_caller(frame): - """Get the first calling frame that is outside of rclpy.""" - global _populate_internal_callers - global _internal_callers - if _populate_internal_callers: - # Populate the list of internal filenames from which logging methods can be called. - # This has to be done from within a function to avoid cyclic module imports. - import rclpy.logging - # Extend the list to preserve any filenames that may have been added by third parties. - # Note: the call to `realpath` will also resolve mixed slashes that can result on Windows. - _internal_callers.extend([ - os.path.realpath(__file__), - os.path.realpath(rclpy.logging.__file__), - ]) - _populate_internal_callers = False - - file_path = os.path.realpath(inspect.getframeinfo(frame).filename) - while any(f in file_path for f in _internal_callers): - frame = frame.f_back - file_path = os.path.realpath(inspect.getframeinfo(frame).filename) - return frame - - -class CallerId( - namedtuple('CallerId', ['function_name', 'file_path', 'line_number', 'last_index'])): - - def __new__(cls, frame=None): - if not frame: - frame = _find_caller(inspect.currentframe()) - return super(CallerId, cls).__new__( - cls, - function_name=frame.f_code.co_name, - file_path=os.path.abspath(inspect.getframeinfo(frame).filename), - line_number=frame.f_lineno, - last_index=frame.f_lasti, # To distinguish between two callers on the same line - ) - - -class LoggingFilter: - """Base class for logging filters.""" - - """ - Parameters of a filter and their default value, if appropriate. - - A default value of None makes a parameter required. - """ - params = {} - - """ - Initialize the context of a logging call, e.g. declare variables needed for - determining the log condition and add them to the context. - """ - @classmethod - def initialize_context(cls, context, **kwargs): - # Store all parameters in the context so we can check that users never try to change them. - for param in cls.params: - context[param] = kwargs.get(param, cls.params[param]) - if context[param] is None: - raise TypeError( - 'Required parameter "{0}" was not specified for logging filter "{1}"' - .format(param, cls.__name__)) - - """ - Decide if it's appropriate to log given a context, and update the context accordingly. - """ - @staticmethod - def should_log(context): - return True - - -class Once(LoggingFilter): - """Ignore all log calls except the first one.""" - - params = { - 'once': None, - } - - @classmethod - def initialize_context(cls, context, **kwargs): - super(Once, cls).initialize_context(context, **kwargs) - context['has_been_logged_once'] = False - - @staticmethod - def should_log(context): - logging_condition = False - if not context['has_been_logged_once']: - logging_condition = True - context['has_been_logged_once'] = True - return logging_condition - - -class Throttle(LoggingFilter): - """Ignore log calls if the last call is not longer ago than the specified duration.""" - - params = { - 'throttle_duration_sec': None, - 'throttle_time_source_type': Clock(), - } - - @classmethod - def initialize_context(cls, context, **kwargs): - super(Throttle, cls).initialize_context(context, **kwargs) - context['throttle_last_logged'] = 0 - if not isinstance(context['throttle_time_source_type'], Clock): - raise ValueError( - 'Received throttle_time_source_type of "{0}" ' - 'is not a clock instance' - .format(context['throttle_time_source_type'])) - - @staticmethod - def should_log(context): - logging_condition = True - now = context['throttle_time_source_type'].now().nanoseconds - next_log_time = context['throttle_last_logged'] + (context['throttle_duration_sec'] * 1e+9) - logging_condition = now >= next_log_time - if logging_condition: - context['throttle_last_logged'] = now - return logging_condition - - -class SkipFirst(LoggingFilter): - """Ignore the first log call but process all subsequent calls.""" - - params = { - 'skip_first': None, - } - - @classmethod - def initialize_context(cls, context, **kwargs): - super(SkipFirst, cls).initialize_context(context, **kwargs) - context['first_has_been_skipped'] = False - - @staticmethod - def should_log(context): - logging_condition = True - if not context['first_has_been_skipped']: - logging_condition = False - context['first_has_been_skipped'] = True - return logging_condition - - -# The ordering of this dictionary defines the order in which filters will be processed. -supported_filters = OrderedDict() -supported_filters['throttle'] = Throttle -supported_filters['skip_first'] = SkipFirst -supported_filters['once'] = Once - - -def get_filters_from_kwargs(**kwargs): - """ - Determine which filters have had parameters specified in the given keyword arguments. - - Returns the list of filters using the order specified by `supported_filters`. - """ - detected_filters = [] - all_supported_params = [] - for supported_filter, filter_class in supported_filters.items(): - filter_params = filter_class.params.keys() - all_supported_params.extend(filter_params) - if any(kwargs.get(param_name) for param_name in filter_params): - detected_filters.append(supported_filter) - # Check that all required parameters (with no default value) have been specified - for detected_filter in detected_filters: - for param_name, default_value in supported_filters[detected_filter].params.items(): - if param_name in kwargs: - continue - - # Param not specified; use the default. - if default_value is None: - raise TypeError( - 'required parameter "{0}" not specified ' - 'but is required for the the logging filter "{1}"'.format( - param_name, detected_filter)) - kwargs[param_name] = default_value - for kwarg in kwargs: - if kwarg not in all_supported_params: - raise TypeError( - 'parameter "{0}" is not one of the recognized logging options "{1}"' - .format(kwarg, all_supported_params) - ) - return detected_filters - - -class RcutilsLogger: - - def __init__(self, name=''): - self.name = name - self.contexts = {} - - def get_child(self, name): - if not name: - raise ValueError('Child logger name must not be empty.') - if self.name: - # Prepend the name of this logger - name = self.name + '.' + name - return RcutilsLogger(name=name) - - def set_level(self, level): - level = LoggingSeverity(level) - return _rclpy.rclpy_logging_set_logger_level(self.name, level) - - def get_effective_level(self): - level = LoggingSeverity( - _rclpy.rclpy_logging_get_logger_effective_level(self.name)) - return level - - def is_enabled_for(self, severity): - severity = LoggingSeverity(severity) - return _rclpy.rclpy_logging_logger_is_enabled_for(self.name, severity) - - def log(self, message, severity, **kwargs): - r""" - Log a message with the specified severity. - - The message will not be logged if: - * the logger is not enabled for the message's severity (the message severity is less than - the level of the logger), or - * a logging filter causes the message to be skipped. - - .. note:: - Logging filters will only be evaluated if the logger is enabled for the message's - severity. - - :param message str: message to log. - :param severity: severity of the message. - :type severity: :py:class:LoggingSeverity - :keyword name str: name of the logger to use. - :param \**kwargs: optional parameters for logging filters (see below). - - :Keyword Arguments: - * *throttle_duration_sec* (``float``) -- - Duration of the throttle interval for the :py:class:Throttle: filter. - * *throttle_time_source_type* (``str``) -- - Optional time source type for the :py:class:Throttle: filter (default of - ``RCUTILS_STEADY_TIME``) - * *skip_first* (``bool``) -- - If True, enable the :py:class:SkipFirst: filter. - * *once* (``bool``) -- - If True, enable the :py:class:Once: filter. - :returns: False if a filter caused the message to not be logged; True otherwise. - :raises: TypeError on invalid filter parameter combinations. - :raises: ValueError on invalid parameters values. - :rtype: bool - """ - # Gather context info and check filters only if the severity is appropriate. - if not self.is_enabled_for(severity): - return False - - severity = LoggingSeverity(severity) - - name = kwargs.pop('name', self.name) - - # Infer the requested log filters from the keyword arguments - detected_filters = get_filters_from_kwargs(**kwargs) - - # Get/prepare the context corresponding to the caller. - caller_id = CallerId() - if caller_id not in self.contexts: - context = {'name': name, 'severity': severity} - for detected_filter in detected_filters: - if detected_filter in supported_filters: - supported_filters[detected_filter].initialize_context(context, **kwargs) - context['filters'] = detected_filters - self.contexts[caller_id] = context - else: - context = self.contexts[caller_id] - # Don't support any changes to the logger. - if severity != context['severity']: - raise ValueError('Logger severity cannot be changed between calls.') - if name != context['name']: - raise ValueError('Logger name cannot be changed between calls.') - if detected_filters != context['filters']: - raise ValueError('Requested logging filters cannot be changed between calls.') - for detected_filter in detected_filters: - filter_params = supported_filters[detected_filter].params - if any(context[p] != kwargs.get(p, filter_params[p]) for p in filter_params): - raise ValueError( - 'Logging filter parameters cannot be changed between calls.') - - # Check if any filter determines the message shouldn't be processed. - # Note(dhood): even if a message doesn't get logged, a filter might still update its state - # as if it had been. This matches the behavior of the C logging macros provided by rcutils. - for logging_filter in context['filters']: - if not supported_filters[logging_filter].should_log(context): - return False - - # Call the relevant function from the C extension. - _rclpy.rclpy_logging_rcutils_log( - severity, name, message, - caller_id.function_name, caller_id.file_path, caller_id.line_number) - return True - - def debug(self, message, **kwargs): - """Log a message with `DEBUG` severity via :py:classmethod:RcutilsLogger.log:.""" - return self.log(message, LoggingSeverity.DEBUG, **kwargs) - - def info(self, message, **kwargs): - """Log a message with `INFO` severity via :py:classmethod:RcutilsLogger.log:.""" - return self.log(message, LoggingSeverity.INFO, **kwargs) - - def warning(self, message, **kwargs): - """Log a message with `WARN` severity via :py:classmethod:RcutilsLogger.log:.""" - return self.log(message, LoggingSeverity.WARN, **kwargs) - - def warn(self, message, **kwargs): - """ - Log a message with `WARN` severity via :py:classmethod:RcutilsLogger.log:. - - Deprecated in favor of :py:classmethod:RcutilsLogger.warning:. - """ - return self.warning(message, **kwargs) - - def error(self, message, **kwargs): - """Log a message with `ERROR` severity via :py:classmethod:RcutilsLogger.log:.""" - return self.log(message, LoggingSeverity.ERROR, **kwargs) - - def fatal(self, message, **kwargs): - """Log a message with `FATAL` severity via :py:classmethod:RcutilsLogger.log:.""" - return self.log(message, LoggingSeverity.FATAL, **kwargs) diff --git a/src/rclpy/lifecycle/__init__.py b/src/rclpy/lifecycle/__init__.py deleted file mode 100644 index 64f1688..0000000 --- a/src/rclpy/lifecycle/__init__.py +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from .managed_entity import ManagedEntity -from .managed_entity import SimpleManagedEntity -from .node import LifecycleNode -from .node import LifecycleNodeMixin -from .node import LifecycleState -from .publisher import LifecyclePublisher - -from ..impl.implementation_singleton import rclpy_implementation as _rclpy - -# reexport LifecycleNode as Node, so it's possible to write: -# from rclpy.lifecycle import Node -# Do not include that in __all__ to avoid mixing it up with rclpy.node.Node. -Node = LifecycleNode -# same idea here -NodeMixin = LifecycleNodeMixin -State = LifecycleState -Publisher = LifecyclePublisher - -# enum defined in pybind11 plugin -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType - - -__all__ = [ - 'LifecycleNodeMixin', - 'LifecycleNode', - 'LifecycleState', - 'LifecyclePublisher', - 'ManagedEntity', - 'SimpleManagedEntity', -] diff --git a/src/rclpy/lifecycle/managed_entity.py b/src/rclpy/lifecycle/managed_entity.py deleted file mode 100644 index 93aaf9b..0000000 --- a/src/rclpy/lifecycle/managed_entity.py +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from functools import wraps - -from ..impl.implementation_singleton import rclpy_implementation as _rclpy - - -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType - - -class ManagedEntity: - - def on_configure(self, state) -> TransitionCallbackReturn: - """Handle configure transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_cleanup(self, state) -> TransitionCallbackReturn: - """Handle cleanup transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_shutdown(self, state) -> TransitionCallbackReturn: - """Handle shutdown transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_activate(self, state) -> TransitionCallbackReturn: - """Handle activate transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state) -> TransitionCallbackReturn: - """Handle deactivate transition request.""" - return TransitionCallbackReturn.SUCCESS - - def on_error(self, state) -> TransitionCallbackReturn: - """Handle error transition request.""" - return TransitionCallbackReturn.SUCCESS - - -class SimpleManagedEntity(ManagedEntity): - """A simple managed entity that only sets a flag when activated/deactivated.""" - - def __init__(self): - self._enabled = False - - def on_activate(self, state) -> TransitionCallbackReturn: - self._enabled = True - return TransitionCallbackReturn.SUCCESS - - def on_deactivate(self, state) -> TransitionCallbackReturn: - self._enabled = False - return TransitionCallbackReturn.SUCCESS - - @property - def is_activated(self): - return self._enabled - - @staticmethod - def when_enabled(wrapped=None, *, when_not_enabled=None): - def decorator(wrapped): - @wraps(wrapped) - def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs): - if not self._enabled: - if when_not_enabled is not None: - when_not_enabled() - return - wrapped(self, *args, **kwargs) - return only_when_enabled_wrapper - if wrapped is None: - return decorator - return decorator(wrapped) diff --git a/src/rclpy/lifecycle/node.py b/src/rclpy/lifecycle/node.py deleted file mode 100644 index 6528007..0000000 --- a/src/rclpy/lifecycle/node.py +++ /dev/null @@ -1,439 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Callable -from typing import Dict -from typing import NamedTuple -from typing import Optional -from typing import Set - -import lifecycle_msgs.msg -import lifecycle_msgs.srv - -from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile -from rclpy.service import Service -from rclpy.type_support import check_is_valid_srv_type - -from .managed_entity import ManagedEntity -from .publisher import LifecyclePublisher - - -TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType - - -class LifecycleState(NamedTuple): - label: str - state_id: int - - -class LifecycleNodeMixin(ManagedEntity): - """ - Mixin class to share as most code as possible between `Node` and `LifecycleNode`. - - This class is not useful if not used in multiple inheritance together with `Node`, - as it access attributes created by `Node` directly here! - """ - - def __init__( - self, - *, - enable_communication_interface: bool = True, - callback_group: Optional[CallbackGroup] = None, - ): - """ - Initialize a lifecycle node. - - :param enable_communication_interface: Creates the lifecycle nodes services and publisher - if True. - :param callback_group: Callback group that will be used by all the lifecycle - node services. - """ - self._callbacks: Dict[int, Callable[[LifecycleState], TransitionCallbackReturn]] = {} - # register all state machine transition callbacks - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_CONFIGURING, - self.on_configure) - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_CLEANINGUP, - self.on_cleanup) - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_SHUTTINGDOWN, - self.on_shutdown) - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_ACTIVATING, - self.on_activate) - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_DEACTIVATING, - self.on_deactivate) - self.__register_callback( - lifecycle_msgs.msg.State.TRANSITION_STATE_ERRORPROCESSING, - self.on_error) - - if callback_group is None: - callback_group = self.default_callback_group - self._managed_entities: Set[ManagedEntity] = set() - for srv_type in ( - lifecycle_msgs.srv.ChangeState, - lifecycle_msgs.srv.GetState, - lifecycle_msgs.srv.GetAvailableStates, - lifecycle_msgs.srv.GetAvailableTransitions, - ): - # this doesn't only checks, but also imports some stuff we need later - check_is_valid_srv_type(srv_type) - - with self.handle: - self._state_machine = _rclpy.LifecycleStateMachine( - self.handle, enable_communication_interface) - if enable_communication_interface: - self._service_change_state = Service( - self._state_machine.service_change_state, - lifecycle_msgs.srv.ChangeState, - self._state_machine.service_change_state.name, - self.__on_change_state, - callback_group, - QoSProfile(**self._state_machine.service_change_state.qos)) - self._service_get_state = Service( - self._state_machine.service_get_state, - lifecycle_msgs.srv.GetState, - self._state_machine.service_get_state.name, - self.__on_get_state, - callback_group, - QoSProfile(**self._state_machine.service_get_state.qos)) - self._service_get_available_states = Service( - self._state_machine.service_get_available_states, - lifecycle_msgs.srv.GetAvailableStates, - self._state_machine.service_get_available_states.name, - self.__on_get_available_states, - callback_group, - QoSProfile(**self._state_machine.service_get_available_states.qos)) - self._service_get_available_transitions = Service( - self._state_machine.service_get_available_transitions, - lifecycle_msgs.srv.GetAvailableTransitions, - self._state_machine.service_get_available_transitions.name, - self.__on_get_available_transitions, - callback_group, - QoSProfile(**self._state_machine.service_get_available_transitions.qos)) - self._service_get_transition_graph = Service( - self._state_machine.service_get_transition_graph, - lifecycle_msgs.srv.GetAvailableTransitions, - self._state_machine.service_get_transition_graph.name, - self.__on_get_transition_graph, - callback_group, - QoSProfile(**self._state_machine.service_get_transition_graph.qos)) - - lifecycle_services = [ - self._service_change_state, - self._service_get_state, - self._service_get_available_states, - self._service_get_available_transitions, - self._service_get_transition_graph, - ] - for s in lifecycle_services: - callback_group.add_entity(s) - # Extend base class list of services, so they are added to the executor when spinning. - self._services.extend(lifecycle_services) - - def trigger_configure(self): - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE) - - def trigger_cleanup(self): - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CLEANUP) - - def trigger_shutdown(self): - current_state = self._state_machine.current_state[1] - if current_state == 'unconfigured': - return self.__change_state( - lifecycle_msgs.msg.Transition.TRANSITION_UNCONFIGURED_SHUTDOWN) - if current_state == 'inactive': - return self.__change_state( - lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN) - if current_state == 'active': - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN) - raise _rclpy.RCLError('Shutdown transtion not possible') - - def trigger_activate(self): - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE) - - def trigger_deactivate(self): - return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE) - - def add_managed_entity(self, entity: ManagedEntity): - if not isinstance(entity, ManagedEntity): - raise TypeError('Expected a rclpy.lifecycle.ManagedEntity instance.') - self._managed_entities.add(entity) - - def __transition_callback_impl(self, callback_name: str, state: LifecycleState): - for entity in self._managed_entities: - cb = getattr(entity, callback_name) - ret = cb(state) - if not isinstance(ret, TransitionCallbackReturn): - raise TypeError( - f'{callback_name}() return value of class {type(entity)} should be' - ' `TransitionCallbackReturn`.\n' - f'Instance of the class that caused the failure: {entity}') - if ret != TransitionCallbackReturn.SUCCESS: - return ret - return TransitionCallbackReturn.SUCCESS - - def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle a configuring transition. - - This is the default on_configure() callback. - It will call all on_configure() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_configure() in derived classes. - """ - return self.__transition_callback_impl('on_configure', state) - - def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle a cleaning up transition. - - This is the default on_cleanup() callback. - It will call all on_cleanup() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_cleanup() in derived classes. - """ - return self.__transition_callback_impl('on_cleanup', state) - - def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle a shutting down transition. - - This is the default on_shutdown() callback. - It will call all on_shutdown() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_shutdown() in derived classes. - """ - return self.__transition_callback_impl('on_shutdown', state) - - def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle an activating transition. - - This is the default on_activate() callback. - It will call all on_activate() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_activate() in derived classes. - """ - return self.__transition_callback_impl('on_activate', state) - - def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle a deactivating transition. - - This is the default on_deactivate() callback. - It will call all on_deactivate() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_deactivate() in derived classes. - """ - return self.__transition_callback_impl('on_deactivate', state) - - def on_error(self, state: LifecycleState) -> TransitionCallbackReturn: - """ - Handle a transition error. - - This is the default on_error() callback. - It will call all on_error() callbacks of managed entities, giving up at the first - entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR. - - It's possible to override this callback if the default behavior is not desired. - If you only want to extend what this callback does, make sure to call - super().on_error() in derived classes. - """ - return self.__transition_callback_impl('on_error', state) - - def create_lifecycle_publisher(self, *args, **kwargs): - # TODO(ivanpauno): Should we override lifecycle publisher? - # There is an issue with python using the overridden method - # when creating publishers for builitin publishers (like parameters events). - # We could override them after init, similar to what we do to override publish() - # in LifecycleNode. - # Having both options seem fine. - if 'publisher_class' in kwargs: - raise TypeError( - "create_publisher() got an unexpected keyword argument 'publisher_class'") - pub = Node.create_publisher(self, *args, **kwargs, publisher_class=LifecyclePublisher) - self._managed_entities.add(pub) - return pub - - def destroy_lifecycle_publisher(self, publisher: LifecyclePublisher): - try: - self._managed_entities.remove(publisher) - except KeyError: - pass - return Node.destroy_publisher(self, publisher) - - def __register_callback( - self, state_id: int, callback: Callable[[LifecycleState], TransitionCallbackReturn] - ) -> bool: - """ - Register a callback that will be triggered when transitioning to state_id. - - The registered callback takes as an argument the previous state id, and returns - a TransitionCallbackReturn value. - """ - self._callbacks[state_id] = callback - return True - - def __execute_callback( - self, current_state_id: int, previous_state: LifecycleState - ) -> TransitionCallbackReturn: - cb = self._callbacks.get(current_state_id, None) - if cb is None: - return TransitionCallbackReturn.SUCCESS - try: - ret = cb(previous_state) - return ret - except Exception: - # TODO(ivanpauno): log sth here - return TransitionCallbackReturn.ERROR - - def __change_state(self, transition_id: int) -> TransitionCallbackReturn: - self.__check_is_initialized() - initial_state = self._state_machine.current_state - initial_state = LifecycleState(state_id=initial_state[0], label=initial_state[1]) - self._state_machine.trigger_transition_by_id(transition_id, True) - - cb_return_code = self.__execute_callback( - self._state_machine.current_state[0], initial_state) - self._state_machine.trigger_transition_by_label(cb_return_code.to_label(), True) - - if cb_return_code == TransitionCallbackReturn.ERROR: - # Now we're in the errorprocessing state, trigger the on_error callback - # and transition again based on the return code. - error_cb_ret_code = self.__execute_callback( - self._state_machine.current_state[0], initial_state) - self._state_machine.trigger_transition_by_label(error_cb_ret_code.to_label(), True) - return cb_return_code - - def __check_is_initialized(self): - if not self._state_machine.initialized: - raise RuntimeError( - 'Internal error: got service request while lifecycle state machine ' - 'is not initialized.') - - def __on_change_state( - self, - req: lifecycle_msgs.srv.ChangeState.Request, - resp: lifecycle_msgs.srv.ChangeState.Response - ): - self.__check_is_initialized() - transition_id = req.transition.id - if req.transition.label: - try: - transition_id = self._state_machine.get_transition_by_label(req.transition.label) - except _rclpy.RCLError: - resp.success = False - return resp - cb_return_code = self.__change_state(transition_id) - resp.success = cb_return_code == TransitionCallbackReturn.SUCCESS - return resp - - def __on_get_state( - self, - req: lifecycle_msgs.srv.GetState.Request, - resp: lifecycle_msgs.srv.GetState.Response - ): - self.__check_is_initialized() - resp.current_state.id, resp.current_state.label = self._state_machine.current_state - return resp - - def __on_get_available_states( - self, - req: lifecycle_msgs.srv.GetAvailableStates.Request, - resp: lifecycle_msgs.srv.GetAvailableStates.Response - ): - self.__check_is_initialized() - for state_id, label in self._state_machine.available_states: - resp.available_states.append(lifecycle_msgs.msg.State(id=state_id, label=label)) - return resp - - def __on_get_available_transitions( - self, - req: lifecycle_msgs.srv.GetAvailableTransitions.Request, - resp: lifecycle_msgs.srv.GetAvailableTransitions.Response - ): - self.__check_is_initialized() - for transition_description in self._state_machine.available_transitions: - transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ - transition_description - item = lifecycle_msgs.msg.TransitionDescription() - item.transition.id = transition_id - item.transition.label = transition_label - item.start_state.id = start_id - item.start_state.label = start_label - item.goal_state.id = goal_id - item.goal_state.label = goal_label - resp.available_transitions.append(item) - return resp - - def __on_get_transition_graph( - self, - req: lifecycle_msgs.srv.GetAvailableTransitions.Request, - resp: lifecycle_msgs.srv.GetAvailableTransitions.Response - ): - self.__check_is_initialized() - for transition_description in self._state_machine.transition_graph: - transition_id, transition_label, start_id, start_label, goal_id, goal_label = \ - transition_description - item = lifecycle_msgs.msg.TransitionDescription() - item.transition.id = transition_id - item.transition.label = transition_label - item.start_state.id = start_id - item.start_state.label = start_label - item.goal_state.id = goal_id - item.goal_state.label = goal_label - resp.available_transitions.append(item) - return resp - - -class LifecycleNode(LifecycleNodeMixin, Node): - """ - A ROS 2 managed node. - - This class extends Node with the methods provided by LifecycleNodeMixin. - Methods in LifecycleNodeMixin override the ones in Node. - """ - - def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs): - """ - Create a lifecycle node. - - See rclpy.lifecycle.LifecycleNodeMixin.__init__() and rclpy.node.Node() - for the documentation of each parameter. - """ - Node.__init__(self, node_name, **kwargs) - LifecycleNodeMixin.__init__( - self, - enable_communication_interface=enable_communication_interface) diff --git a/src/rclpy/lifecycle/publisher.py b/src/rclpy/lifecycle/publisher.py deleted file mode 100644 index 7c31868..0000000 --- a/src/rclpy/lifecycle/publisher.py +++ /dev/null @@ -1,37 +0,0 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Union - -from rclpy.publisher import MsgType -from rclpy.publisher import Publisher - -from .managed_entity import SimpleManagedEntity - - -class LifecyclePublisher(SimpleManagedEntity, Publisher): - """Managed publisher entity.""" - - def __init__(self, *args, **kwargs): - SimpleManagedEntity.__init__(self) - Publisher.__init__(self, *args, **kwargs) - - @SimpleManagedEntity.when_enabled - def publish(self, msg: Union[MsgType, bytes]) -> None: - """ - Publish a message if the lifecycle publisher is enabled. - - See rclpy.publisher.Publisher.publish() for more details. - """ - Publisher.publish(self, msg) diff --git a/src/rclpy/logging.py b/src/rclpy/logging.py deleted file mode 100644 index bce1703..0000000 --- a/src/rclpy/logging.py +++ /dev/null @@ -1,68 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -from pathlib import Path - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.impl.logging_severity import LoggingSeverity -import rclpy.impl.rcutils_logger - - -_root_logger = rclpy.impl.rcutils_logger.RcutilsLogger() - - -def get_logger(name): - if not name: - raise ValueError('Logger name must not be empty.') - return _root_logger.get_child(name) - - -def initialize(): - return _rclpy.rclpy_logging_initialize() - - -def shutdown(): - return _rclpy.rclpy_logging_shutdown() - - -def clear_config(): - """Clear the configuration of the logging system, e.g. logger levels.""" - shutdown() - initialize() - - -def set_logger_level(name, level): - level = LoggingSeverity(level) - return _rclpy.rclpy_logging_set_logger_level(name, level) - - -def get_logger_effective_level(name): - logger_level = _rclpy.rclpy_logging_get_logger_effective_level(name) - return LoggingSeverity(logger_level) - - -def get_logging_severity_from_string(log_severity): - return LoggingSeverity( - _rclpy.rclpy_logging_severity_level_from_string(log_severity)) - - -def get_logging_directory() -> Path: - """ - Return the current logging directory being used. - - For more details, see .. c:function:: - rcl_logging_ret_t rcl_logging_get_logging_directory(rcutils_allocator_t, char **) - """ - return Path(_rclpy.rclpy_logging_get_logging_directory()) diff --git a/src/rclpy/node.py b/src/rclpy/node.py deleted file mode 100644 index b996487..0000000 --- a/src/rclpy/node.py +++ /dev/null @@ -1,1951 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import math - -from typing import Any -from typing import Callable -from typing import Dict -from typing import Iterator -from typing import List -from typing import Optional -from typing import Sequence -from typing import Tuple -from typing import Type -from typing import TypeVar -from typing import Union - -import warnings -import weakref - -from rcl_interfaces.msg import FloatingPointRange -from rcl_interfaces.msg import IntegerRange -from rcl_interfaces.msg import Parameter as ParameterMsg -from rcl_interfaces.msg import ParameterDescriptor -from rcl_interfaces.msg import ParameterEvent -from rcl_interfaces.msg import ParameterType -from rcl_interfaces.msg import ParameterValue -from rcl_interfaces.msg import SetParametersResult - -from rclpy.callback_groups import CallbackGroup -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.client import Client -from rclpy.clock import Clock -from rclpy.clock import ROSClock -from rclpy.constants import S_TO_NS -from rclpy.context import Context -from rclpy.exceptions import InvalidHandle -from rclpy.exceptions import InvalidParameterTypeException -from rclpy.exceptions import InvalidParameterValueException -from rclpy.exceptions import InvalidTopicNameException -from rclpy.exceptions import NotInitializedException -from rclpy.exceptions import ParameterAlreadyDeclaredException -from rclpy.exceptions import ParameterImmutableException -from rclpy.exceptions import ParameterNotDeclaredException -from rclpy.exceptions import ParameterUninitializedException -from rclpy.executors import Executor -from rclpy.expand_topic_name import expand_topic_name -from rclpy.guard_condition import GuardCondition -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -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 qos_profile_parameter_events -from rclpy.qos import qos_profile_services_default -from rclpy.qos import QoSProfile -from rclpy.qos_event import PublisherEventCallbacks -from rclpy.qos_event import SubscriptionEventCallbacks -from rclpy.qos_overriding_options import _declare_qos_parameters -from rclpy.qos_overriding_options import QoSOverridingOptions -from rclpy.service import Service -from rclpy.subscription import Subscription -from rclpy.time_source import TimeSource -from rclpy.timer import Rate -from rclpy.timer import Timer -from rclpy.topic_endpoint_info import TopicEndpointInfo -from rclpy.type_support import check_is_valid_msg_type -from rclpy.type_support import check_is_valid_srv_type -from rclpy.utilities import get_default_context -from rclpy.validate_full_topic_name import validate_full_topic_name -from rclpy.validate_namespace import validate_namespace -from rclpy.validate_node_name import validate_node_name -from rclpy.validate_parameter_name import validate_parameter_name -from rclpy.validate_topic_name import validate_topic_name -from rclpy.waitable import Waitable - -HIDDEN_NODE_PREFIX = '_' - -# Used for documentation purposes only -MsgType = TypeVar('MsgType') -SrvType = TypeVar('SrvType') -SrvTypeRequest = TypeVar('SrvTypeRequest') -SrvTypeResponse = TypeVar('SrvTypeResponse') - -# Re-export exception defined in _rclpy C extension. -# `Node.get_*_names_and_types_by_node` methods may raise this error. -NodeNameNonExistentError = _rclpy.NodeNameNonExistentError - - -class Node: - """ - A Node in the ROS graph. - - A Node is the primary entrypoint in a ROS system for communication. - It can be used to create ROS entities such as publishers, subscribers, services, etc. - """ - - PARAM_REL_TOL = 1e-6 - """ - Relative tolerance for floating point parameter values' comparison. - See `math.isclose` documentation. - """ - - def __init__( - self, - node_name: str, - *, - context: Optional[Context] = None, - cli_args: Optional[List[str]] = None, - namespace: Optional[str] = None, - use_global_arguments: bool = True, - enable_rosout: bool = True, - start_parameter_services: bool = True, - parameter_overrides: Optional[List[Parameter]] = None, - allow_undeclared_parameters: bool = False, - automatically_declare_parameters_from_overrides: bool = False - ) -> None: - """ - Create a Node. - - :param node_name: A name to give to this node. Validated by :func:`validate_node_name`. - :param context: The context to be associated with, or ``None`` for the default global - context. - :param cli_args: A list of strings of command line args to be used only by this node. - These arguments are used to extract remappings used by the node and other ROS specific - settings, as well as user defined non-ROS arguments. - :param namespace: The namespace to which relative topic and service names will be prefixed. - Validated by :func:`validate_namespace`. - :param use_global_arguments: ``False`` if the node should ignore process-wide command line - args. - :param enable_rosout: ``False`` if the node should ignore rosout logging. - :param start_parameter_services: ``False`` if the node should not create parameter - services. - :param parameter_overrides: A list of overrides for initial values for parameters declared - on the node. - :param allow_undeclared_parameters: True if undeclared parameters are allowed. - This flag affects the behavior of parameter-related operations. - :param automatically_declare_parameters_from_overrides: If True, the "parameter overrides" - will be used to implicitly declare parameters on the node during creation. - """ - self.__handle = None - self._context = get_default_context() if context is None else context - self._parameters: dict = {} - self._publishers: List[Publisher] = [] - self._subscriptions: List[Subscription] = [] - self._clients: List[Client] = [] - self._services: List[Service] = [] - self._timers: List[Timer] = [] - self._guards: List[GuardCondition] = [] - self.__waitables: List[Waitable] = [] - self._default_callback_group = MutuallyExclusiveCallbackGroup() - self._parameters_callbacks: List[Callable[[List[Parameter]], SetParametersResult]] = [] - self._rate_group = ReentrantCallbackGroup() - self._allow_undeclared_parameters = allow_undeclared_parameters - self._parameter_overrides = {} - self._descriptors = {} - - namespace = namespace or '' - if not self._context.ok(): - raise NotInitializedException('cannot create node') - with self._context.handle: - try: - self.__node = _rclpy.Node( - node_name, - namespace, - self._context.handle, - cli_args, - use_global_arguments, - enable_rosout - ) - except ValueError: - # these will raise more specific errors if the name or namespace is bad - validate_node_name(node_name) - # emulate what rcl_node_init() does to accept '' and relative namespaces - if not namespace: - namespace = '/' - if not namespace.startswith('/'): - namespace = '/' + namespace - validate_namespace(namespace) - # Should not get to this point - raise RuntimeError('rclpy_create_node failed for unknown reason') - with self.handle: - self._logger = get_logger(self.__node.logger_name()) - - self.__executor_weakref = None - - self._parameter_event_publisher = self.create_publisher( - ParameterEvent, '/parameter_events', qos_profile_parameter_events) - - with self.handle: - self._parameter_overrides = self.__node.get_parameters(Parameter) - # 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: - self._parameter_overrides.update({p.name: p for p in parameter_overrides}) - - # Clock that has support for ROS time. - self._clock = ROSClock() - - if automatically_declare_parameters_from_overrides: - self.declare_parameters( - '', - [ - (name, param.value, ParameterDescriptor()) - for name, param in self._parameter_overrides.items()], - ignore_override=True, - ) - - # Init a time source. - # Note: parameter overrides and parameter event publisher need to be ready at this point - # to be able to declare 'use_sim_time' if it was not declared yet. - self._time_source = TimeSource(node=self) - self._time_source.attach_clock(self._clock) - - if start_parameter_services: - self._parameter_service = ParameterService(self) - - @property - def publishers(self) -> Iterator[Publisher]: - """Get publishers that have been created on this node.""" - yield from self._publishers - - @property - def subscriptions(self) -> Iterator[Subscription]: - """Get subscriptions that have been created on this node.""" - yield from self._subscriptions - - @property - def clients(self) -> Iterator[Client]: - """Get clients that have been created on this node.""" - yield from self._clients - - @property - def services(self) -> Iterator[Service]: - """Get services that have been created on this node.""" - yield from self._services - - @property - def timers(self) -> Iterator[Timer]: - """Get timers that have been created on this node.""" - yield from self._timers - - @property - def guards(self) -> Iterator[GuardCondition]: - """Get guards that have been created on this node.""" - yield from self._guards - - @property - def waitables(self) -> Iterator[Waitable]: - """Get waitables that have been created on this node.""" - yield from self.__waitables - - @property - def executor(self) -> Optional[Executor]: - """Get the executor if the node has been added to one, else return ``None``.""" - if self.__executor_weakref: - return self.__executor_weakref() - return None - - @executor.setter - def executor(self, new_executor: Executor) -> None: - """Set or change the executor the node belongs to.""" - current_executor = self.executor - if current_executor == new_executor: - return - if current_executor is not None: - current_executor.remove_node(self) - if new_executor is None: - self.__executor_weakref = None - else: - new_executor.add_node(self) - self.__executor_weakref = weakref.ref(new_executor) - - def _wake_executor(self): - executor = self.executor - if executor: - executor.wake() - - @property - def context(self) -> Context: - """Get the context associated with the node.""" - return self._context - - @property - def default_callback_group(self) -> CallbackGroup: - """ - Get the default callback group. - - If no other callback group is provided when the a ROS entity is created with the node, - then it is added to the default callback group. - """ - return self._default_callback_group - - @property - def handle(self): - """ - Get the handle to the underlying `rcl_node_t`. - - Cannot be modified after node creation. - - :raises: AttributeError if modified after creation. - """ - return self.__node - - @handle.setter - def handle(self, value): - raise AttributeError('handle cannot be modified after node creation') - - def get_name(self) -> str: - """Get the name of the node.""" - with self.handle: - return self.handle.get_node_name() - - def get_namespace(self) -> str: - """Get the namespace of the node.""" - with self.handle: - return self.handle.get_namespace() - - def get_clock(self) -> Clock: - """Get the clock used by the node.""" - return self._clock - - def get_logger(self): - """Get the nodes logger.""" - return self._logger - - def declare_parameter( - self, - name: str, - value: Any = None, - descriptor: Optional[ParameterDescriptor] = None, - ignore_override: bool = False - ) -> Parameter: - """ - Declare and initialize a parameter. - - This method, if successful, will result in any callback registered with - :func:`add_on_set_parameters_callback` to be called. - - :param name: Fully-qualified name of the parameter, including its namespace. - :param value: Value of the parameter to declare. - :param descriptor: Descriptor for the parameter to declare. - :param ignore_override: True if overrides shall not be taken into account; False otherwise. - :return: Parameter with the effectively assigned value. - :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. - :raises: InvalidParameterException if the parameter name is invalid. - :raises: InvalidParameterValueException if the registered callback rejects the parameter. - """ - if value is None and descriptor is None: - # Temporal patch so we get deprecation warning if only a name is provided. - args = (name, ) - else: - descriptor = ParameterDescriptor() if descriptor is None else descriptor - args = (name, value, descriptor) - return self.declare_parameters('', [args], ignore_override)[0] - - def declare_parameters( - self, - namespace: str, - parameters: List[Union[ - Tuple[str], - Tuple[str, Parameter.Type], - Tuple[str, Any, ParameterDescriptor], - ]], - ignore_override: bool = False - ) -> List[Parameter]: - """ - Declare a list of parameters. - - The tuples in the given parameter list shall contain the name for each parameter, - optionally providing a value and a descriptor. - For each entry in the list, a parameter with a name of "namespace.name" - will be declared. - The resulting value for each declared parameter will be returned, considering - parameter overrides set upon node creation as the first choice, - or provided parameter values as the second one. - - The name expansion is naive, so if you set the namespace to be "foo.", - then the resulting parameter names will be like "foo..name". - However, if the namespace is an empty string, then no leading '.' will be - placed before each name, which would have been the case when naively - expanding "namespace.name". - This allows you to declare several parameters at once without a namespace. - - This method, if successful, will result in any callback registered with - :func:`add_on_set_parameters_callback` to be called once for each parameter. - If one of those calls fail, an exception will be raised and the remaining parameters will - not be declared. - Parameters declared up to that point will not be undeclared. - - :param namespace: Namespace for parameters. - :param parameters: List of tuples with parameters to declare. - :param ignore_override: True if overrides shall not be taken into account; False otherwise. - :return: Parameter list with the effectively assigned values for each of them. - :raises: ParameterAlreadyDeclaredException if the parameter had already been declared. - :raises: InvalidParameterException if the parameter name is invalid. - :raises: InvalidParameterValueException if the registered callback rejects any parameter. - :raises: TypeError if any tuple in **parameters** does not match the annotated type. - """ - parameter_list = [] - descriptors = {} - for index, parameter_tuple in enumerate(parameters): - if len(parameter_tuple) < 1 or len(parameter_tuple) > 3: - raise TypeError( - 'Invalid parameter tuple length at index {index} in parameters list: ' - '{parameter_tuple}; expecting length between 1 and 3'.format_map(locals()) - ) - - value = None - param_type = None - - # Get the values from the tuple, checking its types. - # Use defaults if the tuple doesn't contain value and / or descriptor. - name = parameter_tuple[0] - second_arg = parameter_tuple[1] if 1 < len(parameter_tuple) else None - descriptor = parameter_tuple[2] if 2 < len(parameter_tuple) else ParameterDescriptor() - - if not isinstance(name, str): - raise TypeError( - f'First element {name} at index {index} in parameters list ' - 'is not a str.') - if not isinstance(descriptor, ParameterDescriptor): - raise TypeError( - f'Third element {descriptor} at index {index} in parameters list ' - 'is not a ParameterDescriptor.' - ) - - if len(parameter_tuple) == 1: - warnings.warn( - f"when declaring parameter named '{name}', " - 'declaring a parameter only providing its name is deprecated. ' - 'You have to either:\n' - '\t- Pass a name and a default value different to "PARAMETER NOT SET"' - ' (and optionally a descriptor).\n' - '\t- Pass a name and a parameter type.\n' - '\t- Pass a name and a descriptor with `dynamic_typing=True') - descriptor.dynamic_typing = True - - if isinstance(second_arg, Parameter.Type): - if second_arg == Parameter.Type.NOT_SET: - raise ValueError( - f'Cannot declare parameter {{{name}}} as statically typed of type NOT_SET') - if descriptor.dynamic_typing is True: - raise ValueError( - f'When declaring parameter {{{name}}} passing a descriptor with' - '`dynamic_typing=True` is not allowed when the parameter type is provided') - descriptor.type = second_arg.value - else: - value = second_arg - if not descriptor.dynamic_typing and value is not None: - # infer type from default value - if not isinstance(value, ParameterValue): - descriptor.type = Parameter.Type.from_parameter_value(value).value - else: - if value.type == ParameterType.PARAMETER_NOT_SET: - raise ValueError( - 'Cannot declare a statically typed parameter with default value ' - 'of type PARAMETER_NOT_SET') - descriptor.type = value.type - - # Get value from parameter overrides, of from tuple if it doesn't exist. - if not ignore_override and name in self._parameter_overrides: - value = self._parameter_overrides[name].value - - if namespace: - name = f'{namespace}.{name}' - - # Note(jubeira): declare_parameters verifies the name, but set_parameters doesn't. - validate_parameter_name(name) - - parameter_list.append(Parameter(name, value=value)) - descriptors.update({name: descriptor}) - - parameters_already_declared = [ - parameter.name for parameter in parameter_list if parameter.name in self._parameters - ] - if any(parameters_already_declared): - raise ParameterAlreadyDeclaredException(parameters_already_declared) - - # Call the callback once for each of the parameters, using method that doesn't - # check whether the parameter was declared beforehand or not. - self._set_parameters( - parameter_list, - descriptors, - raise_on_failure=True, - allow_undeclared_parameters=True - ) - # Don't call get_parameters() to bypass check for NOT_SET parameters - return [self._parameters[parameter.name] for parameter in parameter_list] - - def undeclare_parameter(self, name: str): - """ - Undeclare a previously declared parameter. - - This method will not cause a callback registered with - :func:`add_on_set_parameters_callback` to be called. - - :param name: Fully-qualified name of the parameter, including its namespace. - :raises: ParameterNotDeclaredException if parameter had not been declared before. - :raises: ParameterImmutableException if the parameter was created as read-only. - """ - if self.has_parameter(name): - if self._descriptors[name].read_only: - raise ParameterImmutableException(name) - else: - del self._parameters[name] - del self._descriptors[name] - else: - raise ParameterNotDeclaredException(name) - - def has_parameter(self, name: str) -> bool: - """Return True if parameter is declared; False otherwise.""" - return name in self._parameters - - def get_parameter_types(self, names: List[str]) -> List[Parameter.Type]: - """ - Get a list of parameter types. - - :param names: Fully-qualified names of the parameters to get, including their namespaces. - :return: The values for the given parameter types. - A default Parameter.Type.NOT_SET will be returned for undeclared parameters - if undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and at least one parameter hadn't been declared beforehand. - """ - if not all(isinstance(name, str) for name in names): - raise TypeError('All names must be instances of type str') - return [self.get_parameter_type(name) for name in names] - - def get_parameter_type(self, name: str) -> Parameter.Type: - """ - Get a parameter type by name. - - :param name: Fully-qualified name of the parameter, including its namespace. - :return: The type for the given parameter name. - A default Parameter.Type.NOT_SET will be returned for an undeclared parameter - if undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and the parameter hadn't been declared beforehand. - """ - if self.has_parameter(name): - return self._parameters[name].type_.value - elif self._allow_undeclared_parameters: - return Parameter.Type.NOT_SET - else: - raise ParameterNotDeclaredException(name) - - def get_parameters(self, names: List[str]) -> List[Parameter]: - """ - Get a list of parameters. - - :param names: Fully-qualified names of the parameters to get, including their namespaces. - :return: The values for the given parameter names. - A default Parameter will be returned for undeclared parameters if - undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and at least one parameter hadn't been declared beforehand. - :raises: ParameterUninitializedException if at least one parameter is statically typed and - uninitialized. - """ - if not all(isinstance(name, str) for name in names): - raise TypeError('All names must be instances of type str') - return [self.get_parameter(name) for name in names] - - def get_parameter(self, name: str) -> Parameter: - """ - Get a parameter by name. - - :param name: Fully-qualified name of the parameter, including its namespace. - :return: The value for the given parameter name. - A default Parameter will be returned for an undeclared parameter if - undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and the parameter hadn't been declared beforehand. - :raises: ParameterUninitializedException if the parameter is statically typed and - uninitialized. - """ - if self.has_parameter(name): - parameter = self._parameters[name] - if ( - parameter.type_ != Parameter.Type.NOT_SET or - self._descriptors[name].dynamic_typing - ): - return self._parameters[name] - # Statically typed, uninitialized parameter - raise ParameterUninitializedException(name) - elif self._allow_undeclared_parameters: - return Parameter(name, Parameter.Type.NOT_SET, None) - else: - raise ParameterNotDeclaredException(name) - - def get_parameter_or( - self, name: str, alternative_value: Optional[Parameter] = None) -> Parameter: - """ - Get a parameter or the alternative value. - - If the alternative value is None, a default Parameter with the given name and NOT_SET - type will be returned if the parameter was not declared. - - :param name: Fully-qualified name of the parameter, including its namespace. - :param alternative_value: Alternative parameter to get if it had not been declared before. - :return: Requested parameter, or alternative value if it hadn't been declared before or is - an uninitialized statically typed parameter. - """ - if alternative_value is None: - alternative_value = Parameter(name, Parameter.Type.NOT_SET) - - if not self.has_parameter(name): - return alternative_value - - # Return alternative for uninitialized parameters - if (self._parameters[name].type_ == Parameter.Type.NOT_SET): - return alternative_value - - return self._parameters[name] - - def get_parameters_by_prefix(self, prefix: str) -> Dict[str, Optional[Union[ - bool, int, float, str, bytes, - Sequence[bool], Sequence[int], Sequence[float], Sequence[str] - ]]]: - """ - Get parameters that have a given prefix in their names as a dictionary. - - The names which are used as keys in the returned dictionary have the prefix removed. - For example, if you use the prefix "foo" and the parameters "foo.ping", "foo.pong" - and "bar.baz" exist, then the returned dictionary will have the keys "ping" and "pong". - Note that the parameter separator is also removed from the parameter name to create the - keys. - - An empty string for the prefix will match all parameters. - - If no parameters with the prefix are found, an empty dictionary will be returned. - - :param prefix: The prefix of the parameters to get. - :return: Dict of parameters with the given prefix. - """ - if prefix: - prefix = prefix + PARAMETER_SEPARATOR_STRING - prefix_len = len(prefix) - return { - param_name[prefix_len:]: param_value - for param_name, param_value in self._parameters.items() - if param_name.startswith(prefix) - } - - def set_parameters(self, parameter_list: List[Parameter]) -> List[SetParametersResult]: - """ - Set parameters for the node, and return the result for the set action. - - If any parameter in the list was not declared beforehand and undeclared parameters are not - allowed for the node, this method will raise a ParameterNotDeclaredException exception. - - Parameters are set in the order they are declared in the list. - If setting a parameter fails due to not being declared, then the - parameters which have already been set will stay set, and no attempt will - be made to set the parameters which come after. - - If undeclared parameters are allowed, then all the parameters will be implicitly - declared before being set even if they were not declared beforehand. - Parameter overrides are ignored by this method. - - If a callback was registered previously with :func:`add_on_set_parameters_callback`, it - will be called prior to setting the parameters for the node, once for each parameter. - If the callback prevents a parameter from being set, then it will be reflected in the - returned result; no exceptions will be raised in this case. - For each successfully set parameter, a :class:`ParameterEvent` message is - published. - - If the value type of the parameter is NOT_SET, and the existing parameter type is - something else, then the parameter will be implicitly undeclared. - - :param parameter_list: The list of parameters to set. - :return: The result for each set action as a list. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and at least one parameter in the list hadn't been declared beforehand. - """ - return self._set_parameters(parameter_list) - - def _set_parameters( - self, - parameter_list: List[Parameter], - descriptors: Optional[Dict[str, ParameterDescriptor]] = None, - raise_on_failure: bool = False, - allow_undeclared_parameters: bool = False - ) -> List[SetParametersResult]: - """ - Set parameters for the node, and return the result for the set action. - - Method for internal usage; applies a setter method for each parameters in the list. - By default it checks if the parameters were declared, raising an exception if at least - one of them was not. - - If a callback was registered previously with :func:`add_on_set_parameters_callback`, it - will be called prior to setting the parameters for the node, once for each parameter. - If the callback doesn't succeed for a given parameter, it won't be set and either an - unsuccessful result will be returned for that parameter, or an exception will be raised - according to `raise_on_failure` flag. - - :param parameter_list: List of parameters to set. - :param descriptors: Descriptors to set to the given parameters. - If descriptors are given, each parameter in the list must have a corresponding one. - :param raise_on_failure: True if InvalidParameterValueException has to be raised when - the user callback rejects a parameter, False otherwise. - :param allow_undeclared_parameters: If False, this method will check for undeclared - parameters for each of the elements in the parameter list. - :return: The result for each set action as a list. - :raises: InvalidParameterValueException if the user-defined callback rejects the - parameter value and raise_on_failure flag is True. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed in this - method and at least one parameter in the list hadn't been declared beforehand. - """ - if descriptors is not None: - assert all(parameter.name in descriptors for parameter in parameter_list) - - results = [] - for param in parameter_list: - if not allow_undeclared_parameters: - self._check_undeclared_parameters([param]) - # If undeclared parameters are allowed, parameters with type NOT_SET shall be stored. - result = self._set_parameters_atomically( - [param], - descriptors, - allow_not_set_type=allow_undeclared_parameters - ) - if raise_on_failure and not result.successful: - if result.reason.startswith('Wrong parameter type'): - raise InvalidParameterTypeException( - param, Parameter.Type(descriptors[param._name].type).name) - raise InvalidParameterValueException(param.name, param.value, result.reason) - results.append(result) - return results - - def set_parameters_atomically(self, parameter_list: List[Parameter]) -> SetParametersResult: - """ - Set the given parameters, all at one time, and then aggregate result. - - If any parameter in the list was not declared beforehand and undeclared parameters are not - allowed for the node, this method will raise a ParameterNotDeclaredException exception. - - Parameters are set all at once. - If setting a parameter fails due to not being declared, then no parameter will be set set. - Either all of the parameters are set or none of them are set. - - If undeclared parameters are allowed for the node, then all the parameters will be - implicitly declared before being set even if they were not declared beforehand. - - If a callback was registered previously with :func:`add_on_set_parameters_callback`, it - will be called prior to setting the parameters for the node only once for all parameters. - If the callback prevents the parameters from being set, then it will be reflected in the - returned result; no exceptions will be raised in this case. - For each successfully set parameter, a :class:`ParameterEvent` message is published. - - If the value type of the parameter is NOT_SET, and the existing parameter type is - something else, then the parameter will be implicitly undeclared. - - :param parameter_list: The list of parameters to set. - :return: Aggregate result of setting all the parameters atomically. - :raises: ParameterNotDeclaredException if undeclared parameters are not allowed, - and at least one parameter in the list hadn't been declared beforehand. - """ - self._check_undeclared_parameters(parameter_list) - return self._set_parameters_atomically(parameter_list) - - def _check_undeclared_parameters(self, parameter_list: List[Parameter]): - """ - Check if parameter list has correct types and was declared beforehand. - - :raises: ParameterNotDeclaredException if at least one parameter in the list was not - declared beforehand. - """ - if not all(isinstance(parameter, Parameter) for parameter in parameter_list): - raise TypeError("parameter must be instance of type '{}'".format(repr(Parameter))) - - undeclared_parameters = ( - param.name for param in parameter_list if param.name not in self._parameters - ) - if (not self._allow_undeclared_parameters and any(undeclared_parameters)): - raise ParameterNotDeclaredException(list(undeclared_parameters)) - - def _set_parameters_atomically( - self, - parameter_list: List[Parameter], - descriptors: Optional[Dict[str, ParameterDescriptor]] = None, - allow_not_set_type: bool = False - ) -> SetParametersResult: - """ - Set the given parameters, all at one time, and then aggregate result. - - This internal method does not reject undeclared parameters. - If :param:`allow_not_set_type` is False, a parameter with type NOT_SET will be undeclared. - - If a callback was registered previously with :func:`add_on_set_parameters_callback`, it - will be called prior to setting the parameters for the node only once for all parameters. - If the callback prevents the parameters from being set, then it will be reflected in the - returned result; no exceptions will be raised in this case. - For each successfully set parameter, a :class:`ParameterEvent` message is - published. - - :param parameter_list: The list of parameters to set. - :param descriptors: New descriptors to apply to the parameters before setting them. - If descriptors are given, each parameter in the list must have a corresponding one. - :param allow_not_set_type: False if parameters with NOT_SET type shall be undeclared, - True if they should be stored despite not having an actual value. - :return: Aggregate result of setting all the parameters atomically. - """ - if descriptors is not None: - # If new descriptors are provided, ensure every parameter has an assigned descriptor - # and do not check for read-only. - assert all(parameter.name in descriptors for parameter in parameter_list) - result = self._apply_descriptors(parameter_list, descriptors, False) - else: - # If new descriptors are not provided, use existing ones and check for read-only. - result = self._apply_descriptors(parameter_list, self._descriptors, True) - - if not result.successful: - return result - elif self._parameters_callbacks: - for callback in self._parameters_callbacks: - result = callback(parameter_list) - if not result.successful: - return result - result = SetParametersResult(successful=True) - - if result.successful: - parameter_event = ParameterEvent() - # Add fully qualified path of node to parameter event - if self.get_namespace() == '/': - parameter_event.node = self.get_namespace() + self.get_name() - else: - parameter_event.node = self.get_namespace() + '/' + self.get_name() - - for param in parameter_list: - # If parameters without type and value are not allowed, they shall be undeclared. - if not allow_not_set_type and Parameter.Type.NOT_SET == param.type_: - # Parameter deleted. (Parameter had value and new value is not set). - parameter_event.deleted_parameters.append(param.to_parameter_msg()) - # Delete any unset parameters regardless of their previous value. - if param.name in self._parameters: - del self._parameters[param.name] - if param.name in self._descriptors: - del self._descriptors[param.name] - else: - # Update descriptors; set a default if it doesn't exist. - # Don't update if it already exists for the current parameter and a new one - # was not specified in this method call. - if descriptors is not None: - self._descriptors[param.name] = descriptors[param.name] - elif param.name not in self._descriptors: - descriptor = ParameterDescriptor() - descriptor.dynamic_typing = True - self._descriptors[param.name] = descriptor - - if Parameter.Type.NOT_SET == self.get_parameter_or(param.name).type_: - # Parameter is new. (Parameter had no value and new value is set) - parameter_event.new_parameters.append(param.to_parameter_msg()) - else: - parameter_event.changed_parameters.append( - param.to_parameter_msg()) - - # Descriptors have already been applied by this point. - self._parameters[param.name] = param - - parameter_event.stamp = self._clock.now().to_msg() - self._parameter_event_publisher.publish(parameter_event) - - return result - - def add_on_set_parameters_callback( - self, - callback: Callable[[List[Parameter]], SetParametersResult] - ) -> None: - """ - Add a callback in front to the list of callbacks. - - Calling this function will add a callback in self._parameter_callbacks list. - - It is considered bad practice to reject changes for "unknown" parameters as this prevents - other parts of the node (that may be aware of these parameters) from handling them. - - :param callback: The function that is called whenever parameters are set for the node. - """ - self._parameters_callbacks.insert(0, callback) - - def remove_on_set_parameters_callback( - self, - callback: Callable[[List[Parameter]], SetParametersResult] - ) -> None: - """ - Remove a callback from list of callbacks. - - Calling this function will remove the callback from self._parameter_callbacks list. - - :param callback: The function that is called whenever parameters are set for the node. - :raises: ValueError if a callback is not present in the list of callbacks. - """ - self._parameters_callbacks.remove(callback) - - def _apply_descriptors( - self, - parameter_list: List[Parameter], - descriptors: Dict[str, ParameterDescriptor], - check_read_only: bool = True - ) -> SetParametersResult: - """ - Apply descriptors to parameters and return an aggregated result without saving parameters. - - In case no descriptors are provided to the method, existing descriptors shall be used. - In any case, if a given parameter doesn't have a descriptor it shall be skipped. - - :param parameter_list: Parameters to be checked. - :param descriptors: Descriptors to apply. - :param check_read_only: True if read-only check has to be applied. - :return: SetParametersResult; successful if checks passed, unsuccessful otherwise. - :raises: ParameterNotDeclaredException if a descriptor is not provided, the given parameter - name had not been declared and undeclared parameters are not allowed. - """ - for param in parameter_list: - if param.name in descriptors: - result = self._apply_descriptor(param, descriptors[param.name], check_read_only) - if not result.successful: - return result - return SetParametersResult(successful=True) - - def _apply_descriptor( - self, - parameter: Parameter, - descriptor: Optional[ParameterDescriptor] = None, - check_read_only: bool = True - ) -> SetParametersResult: - """ - Apply a descriptor to a parameter and return a result without saving the parameter. - - This method sets the type in the descriptor to match the parameter type. - If a descriptor is provided, its name will be set to the name of the parameter. - - :param parameter: Parameter to be checked. - :param descriptor: Descriptor to apply. If None, the stored descriptor for the given - parameter's name is used instead. - :param check_read_only: True if read-only check has to be applied. - :return: SetParametersResult; successful if checks passed, unsuccessful otherwise. - :raises: ParameterNotDeclaredException if a descriptor is not provided, the given parameter - name had not been declared and undeclared parameters are not allowed. - """ - if descriptor is None: - descriptor = self.describe_parameter(parameter.name) - else: - descriptor.name = parameter.name - - if check_read_only and descriptor.read_only: - return SetParametersResult( - successful=False, - reason='Trying to set a read-only parameter: {}.'.format(parameter.name)) - - if descriptor.dynamic_typing: - descriptor.type = parameter.type_.value - # If this parameter has already been declared, do not allow undeclaring it - elif self.has_parameter(parameter.name) and parameter.type_ == Parameter.Type.NOT_SET: - return SetParametersResult( - successful=False, - reason='Static parameter cannot be undeclared' - ) - elif ( - parameter.type_ != Parameter.Type.NOT_SET and - parameter.type_.value != descriptor.type - ): - return SetParametersResult( - successful=False, - reason=( - 'Wrong parameter type, expected ' - f"'{Parameter.Type(descriptor.type)}'" - f" got '{parameter.type_}'") - ) - - if parameter.type_ == Parameter.Type.INTEGER and descriptor.integer_range: - return self._apply_integer_range(parameter, descriptor.integer_range[0]) - - if parameter.type_ == Parameter.Type.DOUBLE and descriptor.floating_point_range: - return self._apply_floating_point_range(parameter, descriptor.floating_point_range[0]) - - return SetParametersResult(successful=True) - - def _apply_integer_range( - self, - parameter: Parameter, - integer_range: IntegerRange - ) -> SetParametersResult: - min_value = min(integer_range.from_value, integer_range.to_value) - max_value = max(integer_range.from_value, integer_range.to_value) - - # Values in the edge are always OK. - if parameter.value == min_value or parameter.value == max_value: - return SetParametersResult(successful=True) - - if not min_value < parameter.value < max_value: - return SetParametersResult( - successful=False, - reason='Parameter {} out of range. ' - 'Min: {}, Max: {}, value: {}'.format( - parameter.name, min_value, max_value, parameter.value - ) - ) - - if integer_range.step != 0 and (parameter.value - min_value) % integer_range.step != 0: - return SetParametersResult( - successful=False, - reason='The parameter value for {} is not a valid step. ' - 'Min: {}, max: {}, value: {}, step: {}'.format( - parameter.name, - min_value, - max_value, - parameter.value, - integer_range.step - ) - ) - - return SetParametersResult(successful=True) - - def _apply_floating_point_range( - self, - parameter: Parameter, - floating_point_range: FloatingPointRange - ) -> SetParametersResult: - min_value = min(floating_point_range.from_value, floating_point_range.to_value) - max_value = max(floating_point_range.from_value, floating_point_range.to_value) - - # Values in the edge are always OK. - if ( - math.isclose(parameter.value, min_value, rel_tol=self.PARAM_REL_TOL) or - math.isclose(parameter.value, max_value, rel_tol=self.PARAM_REL_TOL) - ): - return SetParametersResult(successful=True) - - if not min_value < parameter.value < max_value: - return SetParametersResult( - successful=False, - reason='Parameter {} out of range ' - 'Min: {}, Max: {}, value: {}'.format( - parameter.name, min_value, max_value, parameter.value - ) - ) - - if floating_point_range.step != 0.0: - distance_int_steps = round((parameter.value - min_value) / floating_point_range.step) - if not math.isclose( - min_value + distance_int_steps * floating_point_range.step, - parameter.value, - rel_tol=self.PARAM_REL_TOL - ): - return SetParametersResult( - successful=False, - reason='The parameter value for {} is not close enough to a valid step. ' - 'Min: {}, max: {}, value: {}, step: {}'.format( - parameter.name, - min_value, - max_value, - parameter.value, - floating_point_range.step - ) - ) - - return SetParametersResult(successful=True) - - def _apply_descriptor_and_set( - self, - parameter: Parameter, - descriptor: Optional[ParameterDescriptor] = None, - check_read_only: bool = True - ) -> SetParametersResult: - """Apply parameter descriptor and set parameter if successful.""" - result = self._apply_descriptor(parameter, descriptor, check_read_only) - if result.successful: - self._parameters[parameter.name] = parameter - - return result - - def describe_parameter(self, name: str) -> ParameterDescriptor: - """ - Get the parameter descriptor of a given parameter. - - :param name: Fully-qualified name of the parameter, including its namespace. - :return: ParameterDescriptor corresponding to the parameter, - or default ParameterDescriptor if parameter had not been declared before - and undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if parameter had not been declared before - and undeclared parameters are not allowed. - """ - try: - return self._descriptors[name] - except KeyError: - if self._allow_undeclared_parameters: - return ParameterDescriptor() - else: - raise ParameterNotDeclaredException(name) - - def describe_parameters(self, names: List[str]) -> List[ParameterDescriptor]: - """ - Get the parameter descriptors of a given list of parameters. - - :param name: List of fully-qualified names of the parameters to describe. - :return: List of ParameterDescriptors corresponding to the given parameters. - Default ParameterDescriptors shall be returned for parameters that - had not been declared before if undeclared parameters are allowed. - :raises: ParameterNotDeclaredException if at least one parameter - had not been declared before and undeclared parameters are not allowed. - """ - parameter_descriptors = [] - for name in names: - parameter_descriptors.append(self.describe_parameter(name)) - - return parameter_descriptors - - def set_descriptor( - self, - name: str, - descriptor: ParameterDescriptor, - alternative_value: Optional[ParameterValue] = None - ) -> ParameterValue: - """ - Set a new descriptor for a given parameter. - - The name in the descriptor is ignored and set to **name**. - - :param name: Fully-qualified name of the parameter to set the descriptor to. - :param descriptor: New descriptor to apply to the parameter. - :param alternative_value: Value to set to the parameter if the existing value does not - comply with the new descriptor. - :return: ParameterValue for the given parameter name after applying the new descriptor. - :raises: ParameterNotDeclaredException if parameter had not been declared before - and undeclared parameters are not allowed. - :raises: ParameterImmutableException if the parameter exists and is read-only. - :raises: ParameterValueException if neither the existing value nor the alternative value - complies with the provided descriptor. - """ - if not self.has_parameter(name): - if not self._allow_undeclared_parameters: - raise ParameterNotDeclaredException(name) - else: - return self.get_parameter(name).get_parameter_value() - - if self.describe_parameter(name).read_only: - raise ParameterImmutableException(name) - - current_parameter = self.get_parameter(name) - if alternative_value is None: - alternative_parameter = current_parameter - else: - alternative_parameter = Parameter.from_parameter_msg( - ParameterMsg(name=name, value=alternative_value)) - - # First try keeping the parameter, then try the alternative one. - # Don't check for read-only since we are applying a new descriptor now. - if not self._apply_descriptor_and_set(current_parameter, descriptor, False).successful: - alternative_set_result = ( - self._apply_descriptor_and_set(alternative_parameter, descriptor, False) - ) - if not alternative_set_result.successful: - raise InvalidParameterValueException( - name, - alternative_parameter.value, - alternative_set_result.reason - ) - - self._descriptors[name] = descriptor - return self.get_parameter(name).get_parameter_value() - - def _validate_topic_or_service_name(self, topic_or_service_name, *, is_service=False): - name = self.get_name() - namespace = self.get_namespace() - validate_node_name(name) - validate_namespace(namespace) - validate_topic_name(topic_or_service_name, is_service=is_service) - expanded_topic_or_service_name = expand_topic_name(topic_or_service_name, name, namespace) - validate_full_topic_name(expanded_topic_or_service_name, is_service=is_service) - - def _validate_qos_or_depth_parameter(self, qos_or_depth) -> QoSProfile: - if isinstance(qos_or_depth, QoSProfile): - return qos_or_depth - elif isinstance(qos_or_depth, int): - if qos_or_depth < 0: - raise ValueError('history depth must be greater than or equal to zero') - return QoSProfile(depth=qos_or_depth) - else: - raise TypeError( - 'Expected QoSProfile or int, but received {!r}'.format(type(qos_or_depth))) - - def add_waitable(self, waitable: Waitable) -> None: - """ - Add a class that is capable of adding things to the wait set. - - :param waitable: An instance of a waitable that the node will add to the waitset. - """ - self.__waitables.append(waitable) - self._wake_executor() - - def remove_waitable(self, waitable: Waitable) -> None: - """ - Remove a Waitable that was previously added to the node. - - :param waitable: The Waitable to remove. - """ - self.__waitables.remove(waitable) - self._wake_executor() - - def resolve_topic_name(self, topic: str, *, only_expand: bool = False) -> str: - """ - Return a topic name expanded and remapped. - - :param topic: topic name to be expanded and remapped. - :param only_expand: if `True`, remapping rules won't be applied. - :return: a fully qualified topic name, - result of applying expansion and remapping to the given `topic`. - """ - with self.handle: - return _rclpy.rclpy_resolve_name(self.handle, topic, only_expand, False) - - def resolve_service_name( - self, service: str, *, only_expand: bool = False - ) -> str: - """ - Return a service name expanded and remapped. - - :param service: service name to be expanded and remapped. - :param only_expand: if `True`, remapping rules won't be applied. - :return: a fully qualified service name, - result of applying expansion and remapping to the given `service`. - """ - with self.handle: - return _rclpy.rclpy_resolve_name(self.handle, service, only_expand, True) - - def create_publisher( - self, - msg_type, - topic: str, - qos_profile: Union[QoSProfile, int], - *, - callback_group: Optional[CallbackGroup] = None, - event_callbacks: Optional[PublisherEventCallbacks] = None, - qos_overriding_options: Optional[QoSOverridingOptions] = None, - publisher_class: Type[Publisher] = Publisher, - ) -> Publisher: - """ - Create a new publisher. - - :param msg_type: The type of ROS messages the publisher will publish. - :param topic: The name of the topic the publisher will publish to. - :param qos_profile: A QoSProfile or a history depth to apply to the publisher. - In the case that a history depth is provided, the QoS history is set to - KEEP_LAST, the QoS history depth is set to the value - of the parameter, and all other QoS settings are set to their default values. - :param callback_group: The callback group for the publisher's event handlers. - If ``None``, then the node's default callback group is used. - :param event_callbacks: User-defined callbacks for middleware events. - :return: The new publisher. - """ - qos_profile = self._validate_qos_or_depth_parameter(qos_profile) - - callback_group = callback_group or self.default_callback_group - - failed = False - try: - final_topic = self.resolve_topic_name(topic) - except RuntimeError: - # if it's name validation error, raise a more appropriate exception. - try: - self._validate_topic_or_service_name(topic) - except InvalidTopicNameException as ex: - raise ex from None - # else reraise the previous exception - raise - - if qos_overriding_options is None: - qos_overriding_options = QoSOverridingOptions([]) - _declare_qos_parameters( - Publisher, self, final_topic, qos_profile, qos_overriding_options) - - # this line imports the typesupport for the message module if not already done - failed = False - check_is_valid_msg_type(msg_type) - try: - with self.handle: - publisher_object = _rclpy.Publisher( - self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) - except ValueError: - failed = True - if failed: - self._validate_topic_or_service_name(topic) - - try: - publisher = publisher_class( - publisher_object, msg_type, topic, qos_profile, - event_callbacks=event_callbacks or PublisherEventCallbacks(), - callback_group=callback_group) - except Exception: - publisher_object.destroy_when_not_in_use() - raise - self._publishers.append(publisher) - self._wake_executor() - - for event_callback in publisher.event_handlers: - self.add_waitable(event_callback) - - return publisher - - def create_subscription( - self, - msg_type, - topic: str, - callback: Callable[[MsgType], None], - qos_profile: Union[QoSProfile, int], - *, - callback_group: Optional[CallbackGroup] = None, - event_callbacks: Optional[SubscriptionEventCallbacks] = None, - qos_overriding_options: Optional[QoSOverridingOptions] = None, - raw: bool = False - ) -> Subscription: - """ - Create a new subscription. - - :param msg_type: The type of ROS messages the subscription will subscribe to. - :param topic: The name of the topic the subscription will subscribe to. - :param callback: A user-defined callback function that is called when a message is - received by the subscription. - :param qos_profile: A QoSProfile or a history depth to apply to the subscription. - In the case that a history depth is provided, the QoS history is set to - KEEP_LAST, the QoS history depth is set to the value - of the parameter, and all other QoS settings are set to their default values. - :param callback_group: The callback group for the subscription. If ``None``, then the - nodes default callback group is used. - :param event_callbacks: User-defined callbacks for middleware events. - :param raw: If ``True``, then received messages will be stored in raw binary - representation. - """ - qos_profile = self._validate_qos_or_depth_parameter(qos_profile) - - callback_group = callback_group or self.default_callback_group - - try: - final_topic = self.resolve_topic_name(topic) - except RuntimeError: - # if it's name validation error, raise a more appropriate exception. - try: - self._validate_topic_or_service_name(topic) - except InvalidTopicNameException as ex: - raise ex from None - # else reraise the previous exception - raise - - if qos_overriding_options is None: - qos_overriding_options = QoSOverridingOptions([]) - _declare_qos_parameters( - Subscription, self, final_topic, qos_profile, qos_overriding_options) - - # this line imports the typesupport for the message module if not already done - failed = False - check_is_valid_msg_type(msg_type) - try: - with self.handle: - subscription_object = _rclpy.Subscription( - self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) - except ValueError: - failed = True - if failed: - self._validate_topic_or_service_name(topic) - - try: - subscription = Subscription( - subscription_object, msg_type, - topic, callback, callback_group, qos_profile, raw, - event_callbacks=event_callbacks or SubscriptionEventCallbacks()) - except Exception: - subscription_object.destroy_when_not_in_use() - raise - callback_group.add_entity(subscription) - self._subscriptions.append(subscription) - self._wake_executor() - - for event_handler in subscription.event_handlers: - self.add_waitable(event_handler) - - return subscription - - def create_client( - self, - srv_type, - srv_name: str, - *, - qos_profile: QoSProfile = qos_profile_services_default, - callback_group: Optional[CallbackGroup] = None - ) -> Client: - """ - Create a new service client. - - :param srv_type: The service type. - :param srv_name: The name of the service. - :param qos_profile: The quality of service profile to apply the service client. - :param callback_group: The callback group for the service client. If ``None``, then the - nodes default callback group is used. - """ - if callback_group is None: - callback_group = self.default_callback_group - check_is_valid_srv_type(srv_type) - failed = False - try: - with self.handle: - client_impl = _rclpy.Client( - self.handle, - srv_type, - srv_name, - qos_profile.get_c_qos_profile()) - except ValueError: - failed = True - if failed: - self._validate_topic_or_service_name(srv_name, is_service=True) - - client = Client( - self.context, - client_impl, srv_type, srv_name, qos_profile, - callback_group) - callback_group.add_entity(client) - self._clients.append(client) - self._wake_executor() - return client - - def create_service( - self, - srv_type, - srv_name: str, - callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], - *, - qos_profile: QoSProfile = qos_profile_services_default, - callback_group: Optional[CallbackGroup] = None - ) -> Service: - """ - Create a new service server. - - :param srv_type: The service type. - :param srv_name: The name of the service. - :param callback: A user-defined callback function that is called when a service request - received by the server. - :param qos_profile: The quality of service profile to apply the service server. - :param callback_group: The callback group for the service server. If ``None``, then the - nodes default callback group is used. - """ - if callback_group is None: - callback_group = self.default_callback_group - check_is_valid_srv_type(srv_type) - failed = False - try: - with self.handle: - service_impl = _rclpy.Service( - self.handle, - srv_type, - srv_name, - qos_profile.get_c_qos_profile()) - except ValueError: - failed = True - if failed: - self._validate_topic_or_service_name(srv_name, is_service=True) - - service = Service( - service_impl, - srv_type, srv_name, callback, callback_group, qos_profile) - callback_group.add_entity(service) - self._services.append(service) - self._wake_executor() - return service - - def create_timer( - self, - timer_period_sec: float, - callback: Callable, - callback_group: Optional[CallbackGroup] = None, - clock: Optional[Clock] = None, - ) -> Timer: - """ - Create a new timer. - - The timer will be started and every ``timer_period_sec`` number of seconds the provided - callback function will be called. - - :param timer_period_sec: The period (s) of the timer. - :param callback: A user-defined callback function that is called when the timer expires. - :param callback_group: The callback group for the timer. If ``None``, then the nodes - default callback group is used. - :param clock: The clock which the timer gets time from. - """ - timer_period_nsec = int(float(timer_period_sec) * S_TO_NS) - if callback_group is None: - callback_group = self.default_callback_group - if clock is None: - clock = self._clock - timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context) - - callback_group.add_entity(timer) - self._timers.append(timer) - self._wake_executor() - return timer - - def create_guard_condition( - self, - callback: Callable, - callback_group: Optional[CallbackGroup] = None - ) -> GuardCondition: - """Create a new guard condition.""" - if callback_group is None: - callback_group = self.default_callback_group - guard = GuardCondition(callback, callback_group, context=self.context) - - callback_group.add_entity(guard) - self._guards.append(guard) - self._wake_executor() - return guard - - def create_rate( - self, - frequency: float, - clock: Optional[Clock] = None, - ) -> Rate: - """ - Create a Rate object. - - :param frequency: The frequency the Rate runs at (Hz). - :param clock: The clock the Rate gets time from. - """ - if frequency <= 0: - raise ValueError('frequency must be > 0') - # Create a timer and give it to the rate object - period = 1.0 / frequency - # Rate will set its own callback - callback = None - # Rates get their own group so timing is not messed up by other callbacks - group = self._rate_group - timer = self.create_timer(period, callback, group, clock) - return Rate(timer, context=self.context) - - def destroy_publisher(self, publisher: Publisher) -> bool: - """ - Destroy a publisher created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - if publisher in self._publishers: - self._publishers.remove(publisher) - for event_handler in publisher.event_handlers: - self.__waitables.remove(event_handler) - try: - publisher.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_subscription(self, subscription: Subscription) -> bool: - """ - Destroy a subscription created by the node. - - :return: ``True`` if succesful, ``False`` otherwise. - """ - if subscription in self._subscriptions: - self._subscriptions.remove(subscription) - for event_handler in subscription.event_handlers: - self.__waitables.remove(event_handler) - try: - subscription.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_client(self, client: Client) -> bool: - """ - Destroy a service client created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - if client in self._clients: - self._clients.remove(client) - try: - client.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_service(self, service: Service) -> bool: - """ - Destroy a service server created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - if service in self._services: - self._services.remove(service) - try: - service.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_timer(self, timer: Timer) -> bool: - """ - Destroy a timer created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - if timer in self._timers: - self._timers.remove(timer) - try: - timer.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_guard_condition(self, guard: GuardCondition) -> bool: - """ - Destroy a guard condition created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - if guard in self._guards: - self._guards.remove(guard) - try: - guard.destroy() - except InvalidHandle: - return False - self._wake_executor() - return True - return False - - def destroy_rate(self, rate: Rate) -> bool: - """ - Destroy a Rate object created by the node. - - :return: ``True`` if successful, ``False`` otherwise. - """ - success = self.destroy_timer(rate._timer) - rate.destroy() - return success - - def destroy_node(self): - """ - Destroy the node. - - Frees resources used by the node, including any entities created by the following methods: - - * :func:`create_publisher` - * :func:`create_subscription` - * :func:`create_client` - * :func:`create_service` - * :func:`create_timer` - * :func:`create_guard_condition` - - """ - # Drop extra reference to parameter event publisher. - # It will be destroyed with other publishers below. - self._parameter_event_publisher = None - - # Destroy dependent items eagerly to work around a possible hang - # https://github.com/ros2/build_cop/issues/248 - while self._publishers: - self.destroy_publisher(self._publishers[0]) - while self._subscriptions: - self.destroy_subscription(self._subscriptions[0]) - while self._clients: - self.destroy_client(self._clients[0]) - while self._services: - self.destroy_service(self._services[0]) - while self._timers: - self.destroy_timer(self._timers[0]) - while self._guards: - self.destroy_guard_condition(self._guards[0]) - self.__node.destroy_when_not_in_use() - self._wake_executor() - - def get_publisher_names_and_types_by_node( - self, - node_name: str, - node_namespace: str, - no_demangle: bool = False - ) -> List[Tuple[str, List[str]]]: - """ - Get a list of discovered topics for publishers of a remote node. - - :param node_name: Name of a remote node to get publishers for. - :param node_namespace: Namespace of the remote node. - :param no_demangle: If ``True``, then topic names and types returned will not be demangled. - :return: List of tuples. - The first element of each tuple is the topic name and the second element is a list of - topic types. - :raise NodeNameNonExistentError: If the node wasn't found. - :raise RuntimeError: Unexpected failure. - """ - with self.handle: - return _rclpy.rclpy_get_publisher_names_and_types_by_node( - self.handle, no_demangle, node_name, node_namespace) - - def get_subscriber_names_and_types_by_node( - self, - node_name: str, - node_namespace: str, - no_demangle: bool = False - ) -> List[Tuple[str, List[str]]]: - """ - Get a list of discovered topics for subscriptions of a remote node. - - :param node_name: Name of a remote node to get subscriptions for. - :param node_namespace: Namespace of the remote node. - :param no_demangle: If ``True``, then topic names and types returned will not be demangled. - :return: List of tuples. - The first element of each tuple is the topic name and the second element is a list of - topic types. - :raise NodeNameNonExistentError: If the node wasn't found. - :raise RuntimeError: Unexpected failure. - """ - with self.handle: - return _rclpy.rclpy_get_subscriber_names_and_types_by_node( - self.handle, no_demangle, node_name, node_namespace) - - def get_service_names_and_types_by_node( - self, - node_name: str, - node_namespace: str - ) -> List[Tuple[str, List[str]]]: - """ - Get a list of discovered service server topics for a remote node. - - :param node_name: Name of a remote node to get services for. - :param node_namespace: Namespace of the remote node. - :return: List of tuples. - The first element of each tuple is the service server name - and the second element is a list of service types. - :raise NodeNameNonExistentError: If the node wasn't found. - :raise RuntimeError: Unexpected failure. - """ - with self.handle: - return _rclpy.rclpy_get_service_names_and_types_by_node( - self.handle, node_name, node_namespace) - - def get_client_names_and_types_by_node( - self, - node_name: str, - node_namespace: str - ) -> List[Tuple[str, List[str]]]: - """ - Get a list of discovered service client topics for a remote node. - - :param node_name: Name of a remote node to get service clients for. - :param node_namespace: Namespace of the remote node. - :return: List of tuples. - The fist element of each tuple is the service client name - and the second element is a list of service client types. - :raise NodeNameNonExistentError: If the node wasn't found. - :raise RuntimeError: Unexpected failure. - """ - with self.handle: - return _rclpy.rclpy_get_client_names_and_types_by_node( - self.handle, node_name, node_namespace) - - def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: - """ - Get a list topic names and types for the node. - - :param no_demangle: If ``True``, then topic names and types returned will not be demangled. - :return: List of tuples. - The first element of each tuple is the topic name and the second element is a list of - topic types. - """ - with self.handle: - return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) - - def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: - """ - Get a list of service topics for the node. - - :return: List of tuples. - The first element of each tuple is the service name and the second element is a list of - service types. - """ - with self.handle: - return _rclpy.rclpy_get_service_names_and_types(self.handle) - - def get_node_names(self) -> List[str]: - """ - Get a list of names for discovered nodes. - - :return: List of node names. - """ - with self.handle: - names_ns = self.handle.get_node_names_and_namespaces() - return [n[0] for n in names_ns] - - def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: - """ - Get a list of names and namespaces for discovered nodes. - - :return: List of tuples containing two strings: the node name and node namespace. - """ - with self.handle: - return self.handle.get_node_names_and_namespaces() - - def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, str]]: - """ - Get a list of names, namespaces and enclaves for discovered nodes. - - :return: List of tuples containing three strings: the node name, node namespace - and enclave. - """ - with self.handle: - return self.handle.get_node_names_and_namespaces_with_enclaves() - - def get_fully_qualified_name(self) -> str: - """ - Get the node's fully qualified name. - - :return: Fully qualified node name. - """ - with self.handle: - return self.handle.get_fully_qualified_name() - - def _count_publishers_or_subscribers(self, topic_name, func): - fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) - validate_full_topic_name(fq_topic_name) - with self.handle: - return func(fq_topic_name) - - def count_publishers(self, topic_name: str) -> int: - """ - Return the number of publishers on a given topic. - - `topic_name` may be a relative, private, or fully qualified topic name. - A relative or private topic is expanded using this node's namespace and name. - The queried topic name is not remapped. - - :param topic_name: the topic_name on which to count the number of publishers. - :return: the number of publishers on the topic. - """ - with self.handle: - return self._count_publishers_or_subscribers( - topic_name, self.handle.get_count_publishers) - - def count_subscribers(self, topic_name: str) -> int: - """ - Return the number of subscribers on a given topic. - - `topic_name` may be a relative, private, or fully qualified topic name. - A relative or private topic is expanded using this node's namespace and name. - The queried topic name is not remapped. - - :param topic_name: the topic_name on which to count the number of subscribers. - :return: the number of subscribers on the topic. - """ - with self.handle: - return self._count_publishers_or_subscribers( - topic_name, self.handle.get_count_subscribers) - - def _get_info_by_topic( - self, - topic_name: str, - no_mangle: bool, - func: Callable[[object, str, bool], List[Dict]] - ) -> List[TopicEndpointInfo]: - with self.handle: - if no_mangle: - fq_topic_name = topic_name - else: - fq_topic_name = expand_topic_name( - topic_name, self.get_name(), self.get_namespace()) - validate_full_topic_name(fq_topic_name) - fq_topic_name = _rclpy.rclpy_remap_topic_name(self.handle, fq_topic_name) - - info_dicts = func(self.handle, fq_topic_name, no_mangle) - infos = [TopicEndpointInfo(**x) for x in info_dicts] - return infos - - def get_publishers_info_by_topic( - self, - topic_name: str, - no_mangle: bool = False - ) -> List[TopicEndpointInfo]: - """ - Return a list of publishers on a given topic. - - The returned parameter is a list of TopicEndpointInfo objects, where each will contain - the node name, node namespace, topic type, topic endpoint's GID, and its QoS profile. - - When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic - name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). - When the `no_mangle` parameter is `false`, the provided `topic_name` should follow - ROS topic name conventions. - - `topic_name` may be a relative, private, or fully qualified topic name. - A relative or private topic will be expanded using this node's namespace and name. - The queried `topic_name` is not remapped. - - :param topic_name: the topic_name on which to find the publishers. - :param no_mangle: no_mangle if `true`, `topic_name` needs to be a valid middleware topic - name, otherwise it should be a valid ROS topic name. Defaults to `false`. - :return: a list of TopicEndpointInfo for all the publishers on this topic. - """ - return self._get_info_by_topic( - topic_name, - no_mangle, - _rclpy.rclpy_get_publishers_info_by_topic) - - def get_subscriptions_info_by_topic( - self, - topic_name: str, - no_mangle: bool = False - ) -> List[TopicEndpointInfo]: - """ - Return a list of subscriptions on a given topic. - - The returned parameter is a list of TopicEndpointInfo objects, where each will contain - the node name, node namespace, topic type, topic endpoint's GID, and its QoS profile. - - When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic - name for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). - When the `no_mangle` parameter is `false`, the provided `topic_name` should follow - ROS topic name conventions. - - `topic_name` may be a relative, private, or fully qualified topic name. - A relative or private topic will be expanded using this node's namespace and name. - The queried `topic_name` is not remapped. - - :param topic_name: the topic_name on which to find the subscriptions. - :param no_mangle: no_mangle if `true`, `topic_name` needs to be a valid middleware topic - name, otherwise it should be a valid ROS topic name. Defaults to `false`. - :return: a list of TopicEndpointInfo for all the subscriptions on this topic. - """ - return self._get_info_by_topic( - topic_name, - no_mangle, - _rclpy.rclpy_get_subscriptions_info_by_topic) diff --git a/src/rclpy/parameter.py b/src/rclpy/parameter.py deleted file mode 100644 index daad331..0000000 --- a/src/rclpy/parameter.py +++ /dev/null @@ -1,213 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import array -from enum import Enum - -from rcl_interfaces.msg import Parameter as ParameterMsg -from rcl_interfaces.msg import ParameterType, ParameterValue - -PARAMETER_SEPARATOR_STRING = '.' - - -class Parameter: - - class Type(Enum): - NOT_SET = ParameterType.PARAMETER_NOT_SET - BOOL = ParameterType.PARAMETER_BOOL - INTEGER = ParameterType.PARAMETER_INTEGER - DOUBLE = ParameterType.PARAMETER_DOUBLE - STRING = ParameterType.PARAMETER_STRING - BYTE_ARRAY = ParameterType.PARAMETER_BYTE_ARRAY - BOOL_ARRAY = ParameterType.PARAMETER_BOOL_ARRAY - INTEGER_ARRAY = ParameterType.PARAMETER_INTEGER_ARRAY - DOUBLE_ARRAY = ParameterType.PARAMETER_DOUBLE_ARRAY - STRING_ARRAY = ParameterType.PARAMETER_STRING_ARRAY - - @classmethod - def from_parameter_value(cls, parameter_value): - """ - Get a Parameter.Type from a given variable. - - :return: A Parameter.Type corresponding to the instance type of the given value. - :raises: TypeError if the conversion to a type was not possible. - """ - if parameter_value is None: - return Parameter.Type.NOT_SET - elif isinstance(parameter_value, bool): - return Parameter.Type.BOOL - elif isinstance(parameter_value, int): - return Parameter.Type.INTEGER - elif isinstance(parameter_value, float): - return Parameter.Type.DOUBLE - elif isinstance(parameter_value, str): - return Parameter.Type.STRING - elif isinstance(parameter_value, (list, tuple, array.array)): - if all(isinstance(v, bytes) for v in parameter_value): - return Parameter.Type.BYTE_ARRAY - elif all(isinstance(v, bool) for v in parameter_value): - return Parameter.Type.BOOL_ARRAY - elif all(isinstance(v, int) for v in parameter_value): - return Parameter.Type.INTEGER_ARRAY - elif all(isinstance(v, float) for v in parameter_value): - return Parameter.Type.DOUBLE_ARRAY - elif all(isinstance(v, str) for v in parameter_value): - return Parameter.Type.STRING_ARRAY - else: - raise TypeError( - 'The given value is not a list of one of the allowed types' - f" '{parameter_value}'.") - else: - raise TypeError( - f"The given value is not one of the allowed types '{parameter_value}'.") - - def check(self, parameter_value): - if Parameter.Type.NOT_SET == self: - return parameter_value is None - if Parameter.Type.BOOL == self: - return isinstance(parameter_value, bool) - if Parameter.Type.INTEGER == self: - return isinstance(parameter_value, int) - if Parameter.Type.DOUBLE == self: - return isinstance(parameter_value, float) - if Parameter.Type.STRING == self: - return isinstance(parameter_value, str) - if Parameter.Type.BYTE_ARRAY == self: - return isinstance(parameter_value, (list, tuple)) and \ - all(isinstance(v, bytes) and len(v) == 1 for v in parameter_value) - if Parameter.Type.BOOL_ARRAY == self: - return isinstance(parameter_value, (list, tuple)) and \ - all(isinstance(v, bool) for v in parameter_value) - if Parameter.Type.INTEGER_ARRAY == self: - return isinstance(parameter_value, (list, tuple, array.array)) and \ - all(isinstance(v, int) for v in parameter_value) - if Parameter.Type.DOUBLE_ARRAY == self: - return isinstance(parameter_value, (list, tuple, array.array)) and \ - all(isinstance(v, float) for v in parameter_value) - if Parameter.Type.STRING_ARRAY == self: - return isinstance(parameter_value, (list, tuple)) and \ - all(isinstance(v, str) for v in parameter_value) - return False - - @classmethod - def from_parameter_msg(cls, param_msg): - value = None - type_ = Parameter.Type(value=param_msg.value.type) - if Parameter.Type.BOOL == type_: - value = param_msg.value.bool_value - elif Parameter.Type.INTEGER == type_: - value = param_msg.value.integer_value - elif Parameter.Type.DOUBLE == type_: - value = param_msg.value.double_value - elif Parameter.Type.STRING == type_: - value = param_msg.value.string_value - elif Parameter.Type.BYTE_ARRAY == type_: - value = param_msg.value.byte_array_value - elif Parameter.Type.BOOL_ARRAY == type_: - value = param_msg.value.bool_array_value - elif Parameter.Type.INTEGER_ARRAY == type_: - value = param_msg.value.integer_array_value - elif Parameter.Type.DOUBLE_ARRAY == type_: - value = param_msg.value.double_array_value - elif Parameter.Type.STRING_ARRAY == type_: - value = param_msg.value.string_array_value - return cls(param_msg.name, type_, value) - - def __init__(self, name, type_=None, value=None): - if type_ is None: - # This will raise a TypeError if it is not possible to get a type from the value. - type_ = Parameter.Type.from_parameter_value(value) - - if not isinstance(type_, Parameter.Type): - raise TypeError("type must be an instance of '{}'".format(repr(Parameter.Type))) - - if not type_.check(value): - raise ValueError("Type '{}' and value '{}' do not agree".format(type_, value)) - - self._type_ = type_ - self._name = name - self._value = value - - @property - def name(self): - return self._name - - @property - def type_(self): - return self._type_ - - @property - def value(self): - return self._value - - def get_parameter_value(self): - parameter_value = ParameterValue(type=self.type_.value) - if Parameter.Type.BOOL == self.type_: - parameter_value.bool_value = self.value - elif Parameter.Type.INTEGER == self.type_: - parameter_value.integer_value = self.value - elif Parameter.Type.DOUBLE == self.type_: - parameter_value.double_value = self.value - elif Parameter.Type.STRING == self.type_: - parameter_value.string_value = self.value - elif Parameter.Type.BYTE_ARRAY == self.type_: - parameter_value.byte_array_value = self.value - elif Parameter.Type.BOOL_ARRAY == self.type_: - parameter_value.bool_array_value = self.value - elif Parameter.Type.INTEGER_ARRAY == self.type_: - parameter_value.integer_array_value = self.value - elif Parameter.Type.DOUBLE_ARRAY == self.type_: - parameter_value.double_array_value = self.value - elif Parameter.Type.STRING_ARRAY == self.type_: - parameter_value.string_array_value = self.value - return parameter_value - - def to_parameter_msg(self): - return ParameterMsg(name=self.name, value=self.get_parameter_value()) - - -def parameter_value_to_python(parameter_value: ParameterValue): - """ - Get the value for the Python builtin type from a rcl_interfaces/msg/ParameterValue object. - - Returns the value member of the message based on the ``type`` member. - Returns ``None`` if the parameter is "NOT_SET". - - :param parameter_value: The message to get the value from. - :raises RuntimeError: if the member ``type`` has an unexpected value. - """ - if parameter_value.type == ParameterType.PARAMETER_BOOL: - value = parameter_value.bool_value - elif parameter_value.type == ParameterType.PARAMETER_INTEGER: - value = parameter_value.integer_value - elif parameter_value.type == ParameterType.PARAMETER_DOUBLE: - value = parameter_value.double_value - elif parameter_value.type == ParameterType.PARAMETER_STRING: - value = parameter_value.string_value - elif parameter_value.type == ParameterType.PARAMETER_BYTE_ARRAY: - value = list(parameter_value.byte_array_value) - elif parameter_value.type == ParameterType.PARAMETER_BOOL_ARRAY: - value = list(parameter_value.bool_array_value) - elif parameter_value.type == ParameterType.PARAMETER_INTEGER_ARRAY: - value = list(parameter_value.integer_array_value) - elif parameter_value.type == ParameterType.PARAMETER_DOUBLE_ARRAY: - value = list(parameter_value.double_array_value) - elif parameter_value.type == ParameterType.PARAMETER_STRING_ARRAY: - value = list(parameter_value.string_array_value) - elif parameter_value.type == ParameterType.PARAMETER_NOT_SET: - value = None - else: - raise RuntimeError(f'unexpected parameter type {parameter_value.type}') - - return value diff --git a/src/rclpy/parameter_service.py b/src/rclpy/parameter_service.py deleted file mode 100644 index 111fb46..0000000 --- a/src/rclpy/parameter_service.py +++ /dev/null @@ -1,172 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import weakref - -from rcl_interfaces.msg import SetParametersResult -from rcl_interfaces.srv import DescribeParameters, GetParameters, GetParameterTypes -from rcl_interfaces.srv import ListParameters, SetParameters, SetParametersAtomically -from rclpy.exceptions import ParameterNotDeclaredException, ParameterUninitializedException -from rclpy.parameter import Parameter, PARAMETER_SEPARATOR_STRING -from rclpy.qos import qos_profile_parameters -from rclpy.validate_topic_name import TOPIC_SEPARATOR_STRING - - -class ParameterService: - - def __init__(self, node): - self._node_weak_ref = weakref.ref(node) - nodename = node.get_name() - - describe_parameters_service_name = \ - TOPIC_SEPARATOR_STRING.join((nodename, 'describe_parameters')) - node.create_service( - DescribeParameters, describe_parameters_service_name, - self._describe_parameters_callback, qos_profile=qos_profile_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 - ) - 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 - ) - 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 - ) - 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 - ) - 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 - ) - - def _describe_parameters_callback(self, request, response): - node = self._get_node() - for name in request.names: - try: - descriptor = node.describe_parameter(name) - except ParameterNotDeclaredException: - response.descriptors = node.describe_parameters([]) - return response - response.descriptors.append(descriptor) - return response - - def _get_parameters_callback(self, request, response): - node = self._get_node() - for name in request.names: - try: - param = node.get_parameter(name) - except (ParameterNotDeclaredException, ParameterUninitializedException): - response.values = node.get_parameters([]) - return response - response.values.append(param.get_parameter_value()) - return response - - def _get_parameter_types_callback(self, request, response): - node = self._get_node() - for name in request.names: - try: - value = node.get_parameter_type(name) - except ParameterNotDeclaredException: - response.types = node.get_parameter_types([]) - return response - response.types.append(value) - return response - - def _list_parameters_callback(self, request, response): - names_with_prefixes = [] - node = self._get_node() - for name in node._parameters.keys(): - if PARAMETER_SEPARATOR_STRING in name: - names_with_prefixes.append(name) - continue - elif request.prefixes: - for prefix in request.prefixes: - if name.startswith(prefix): - response.result.names.append(name) - continue - else: - response.result.names.append(name) - if 1 == request.depth: - return response - - if not request.DEPTH_RECURSIVE == request.depth: - names_with_prefixes = filter( - lambda name: - name.count(PARAMETER_SEPARATOR_STRING) < request.depth, names_with_prefixes - ) - for name in names_with_prefixes: - if request.prefixes: - for prefix in request.prefixes: - if name.startswith(prefix + PARAMETER_SEPARATOR_STRING): - response.result.names.append(name) - full_prefix = PARAMETER_SEPARATOR_STRING.join( - name.split(PARAMETER_SEPARATOR_STRING)[0:-1]) - if full_prefix not in response.result.prefixes: - response.result.prefixes.append(full_prefix) - if prefix not in response.result.prefixes: - response.result.prefixes.append(prefix) - else: - prefix = PARAMETER_SEPARATOR_STRING.join( - name.split(PARAMETER_SEPARATOR_STRING)[0:-1]) - if prefix not in response.result.prefixes: - response.result.prefixes.append(prefix) - response.result.names.append(name) - - return response - - def _set_parameters_callback(self, request, response): - node = self._get_node() - for p in request.parameters: - param = Parameter.from_parameter_msg(p) - try: - result = node.set_parameters_atomically([param]) - except ParameterNotDeclaredException as e: - result = SetParametersResult( - successful=False, - reason=str(e) - ) - response.results.append(result) - return response - - def _set_parameters_atomically_callback(self, request, response): - node = self._get_node() - try: - response.result = node.set_parameters_atomically([ - Parameter.from_parameter_msg(p) for p in request.parameters]) - except ParameterNotDeclaredException as e: - response.result = SetParametersResult( - successful=False, - reason=str(e) - ) - return response - - def _get_node(self): - node = self._node_weak_ref() - if node is None: - raise ReferenceError('Expected valid node weak reference') - return node diff --git a/src/rclpy/publisher.py b/src/rclpy/publisher.py deleted file mode 100644 index ce98c47..0000000 --- a/src/rclpy/publisher.py +++ /dev/null @@ -1,123 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import TypeVar, Union - -from rclpy.callback_groups import CallbackGroup -from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import QoSProfile -from rclpy.qos_event import PublisherEventCallbacks -from rclpy.qos_event import QoSEventHandler - -MsgType = TypeVar('MsgType') - - -class Publisher: - - def __init__( - self, - publisher_impl: _rclpy.Publisher, - msg_type: MsgType, - topic: str, - qos_profile: QoSProfile, - event_callbacks: PublisherEventCallbacks, - callback_group: CallbackGroup, - ) -> None: - """ - Create a container for a ROS publisher. - - .. warning:: Users should not create a publisher with this constuctor, instead they should - call :meth:`.Node.create_publisher`. - - A publisher is used as a primary means of communication in a ROS system by publishing - messages on a ROS topic. - - :param publisher_impl: Publisher wrapping the underlying ``rcl_publisher_t`` object. - :param msg_type: The type of ROS messages the publisher will publish. - :param topic: The name of the topic the publisher will publish to. - :param qos_profile: The quality of service profile to apply to the publisher. - """ - self.__publisher = publisher_impl - self.msg_type = msg_type - self.topic = topic - self.qos_profile = qos_profile - - self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, publisher_impl, topic) - - def publish(self, msg: Union[MsgType, bytes]) -> None: - """ - Send a message to the topic for the publisher. - - :param msg: The ROS message to publish. - :raises: TypeError if the type of the passed message isn't an instance - of the provided type when the publisher was constructed. - """ - with self.handle: - if isinstance(msg, self.msg_type): - self.__publisher.publish(msg) - elif isinstance(msg, bytes): - self.__publisher.publish_raw(msg) - else: - raise TypeError('Expected {}, got {}'.format(self.msg_type, type(msg))) - - def get_subscription_count(self) -> int: - """Get the amount of subscribers that this publisher has.""" - with self.handle: - return self.__publisher.get_subscription_count() - - @property - def topic_name(self) -> str: - with self.handle: - return self.__publisher.get_topic_name() - - @property - def handle(self): - return self.__publisher - - def destroy(self): - for handler in self.event_handlers: - handler.destroy() - self.__publisher.destroy_when_not_in_use() - - def assert_liveliness(self) -> None: - """ - Manually assert that this Publisher is alive. - - If the QoS Liveliness policy is set to MANUAL_BY_TOPIC, the - application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. - """ - with self.handle: - _rclpy.rclpy_assert_liveliness(self.handle) - - def wait_for_all_acked(self, timeout: Duration = Duration(seconds=-1)) -> bool: - """ - Wait until all published message data is acknowledged or until the timeout elapses. - - If the timeout is negative then this function will block indefinitely until all published - message data is acknowledged. - If the timeout is 0 then it will check if all published message has been acknowledged - without waiting. - If the timeout is greater than 0 then it will return after that period of time has elapsed - or all published message data is acknowledged. - This function only waits for acknowledgments if the publisher's QOS profile is RELIABLE. - Otherwise this function will immediately return true. - - :param timeout: the duration to wait for all published message data to be acknowledged. - :returns: true if all published message data is acknowledged before the timeout, otherwise - false. - """ - with self.handle: - return self.__publisher.wait_for_all_acked(timeout._duration_handle) diff --git a/src/rclpy/qos.py b/src/rclpy/qos.py deleted file mode 100644 index aa2e3ce..0000000 --- a/src/rclpy/qos.py +++ /dev/null @@ -1,499 +0,0 @@ -# Copyright 2016-2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import Enum -from enum import IntEnum -from typing import Union - -import warnings - -from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -class QoSPolicyKind(IntEnum): - """ - Enum for types of QoS policies that a Publisher or Subscription can set. - - This enum matches the one defined in rmw/incompatible_qos_events_statuses.h - """ - - # TODO(mm3188): obtain these enum values from the rmw layer, instead of hardcoding - INVALID = 1 << 0 - DURABILITY = 1 << 1 - DEADLINE = 1 << 2 - LIVELINESS = 1 << 3 - RELIABILITY = 1 << 4 - HISTORY = 1 << 5 - LIFESPAN = 1 << 6, - DEPTH = 1 << 7, - LIVELINESS_LEASE_DURATION = 1 << 8, - AVOID_ROS_NAMESPACE_CONVENTIONS = 1 << 9, - - -def qos_policy_name_from_kind(policy_kind: Union[QoSPolicyKind, int]): - """Get QoS policy name from QoSPolicyKind enum.""" - return QoSPolicyKind(policy_kind).name - - -class InvalidQoSProfileException(Exception): - """Raised when constructing a QoSProfile with invalid arguments.""" - - def __init__(self, *args): - Exception.__init__(self, 'Invalid QoSProfile', *args) - - -class QoSProfile: - """Define Quality of Service policies.""" - - # default QoS profile not exposed to the user to encourage them to think about QoS settings - __qos_profile_default_dict = \ - _rclpy.rmw_qos_profile_t.predefined('qos_profile_default').to_dict() - - __slots__ = [ - '_history', - '_depth', - '_reliability', - '_durability', - '_lifespan', - '_deadline', - '_liveliness', - '_liveliness_lease_duration', - '_avoid_ros_namespace_conventions', - ] - - def __init__(self, **kwargs): - assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ - 'Invalid arguments passed to constructor: %r' % kwargs.keys() - - if 'history' not in kwargs: - if 'depth' not in kwargs: - raise InvalidQoSProfileException('History and/or depth settings are required.') - kwargs['history'] = QoSHistoryPolicy.KEEP_LAST - - self.history = kwargs.get('history') - - if ( - QoSHistoryPolicy.KEEP_LAST == self.history and - 'depth' not in kwargs - ): - raise InvalidQoSProfileException('History set to KEEP_LAST without a depth setting.') - - self.depth = kwargs.get('depth', QoSProfile.__qos_profile_default_dict['depth']) - self.reliability = kwargs.get( - 'reliability', QoSProfile.__qos_profile_default_dict['reliability']) - self.durability = kwargs.get( - 'durability', QoSProfile.__qos_profile_default_dict['durability']) - self.lifespan = kwargs.get('lifespan', QoSProfile.__qos_profile_default_dict['lifespan']) - self.deadline = kwargs.get('deadline', QoSProfile.__qos_profile_default_dict['deadline']) - self.liveliness = kwargs.get( - 'liveliness', QoSProfile.__qos_profile_default_dict['liveliness']) - self.liveliness_lease_duration = kwargs.get( - 'liveliness_lease_duration', - QoSProfile.__qos_profile_default_dict['liveliness_lease_duration']) - self.avoid_ros_namespace_conventions = kwargs.get( - 'avoid_ros_namespace_conventions', - QoSProfile.__qos_profile_default_dict['avoid_ros_namespace_conventions']) - - @property - def history(self): - """ - Get field 'history'. - - :returns: history attribute - :rtype: QoSHistoryPolicy - """ - return self._history - - @history.setter - def history(self, value): - assert isinstance(value, QoSHistoryPolicy) or isinstance(value, int) - self._history = QoSHistoryPolicy(value) - - @property - def reliability(self): - """ - Get field 'reliability'. - - :returns: reliability attribute - :rtype: QoSReliabilityPolicy - """ - return self._reliability - - @reliability.setter - def reliability(self, value): - assert isinstance(value, QoSReliabilityPolicy) or isinstance(value, int) - self._reliability = QoSReliabilityPolicy(value) - - @property - def durability(self): - """ - Get field 'durability'. - - :returns: durability attribute - :rtype: QoSDurabilityPolicy - """ - return self._durability - - @durability.setter - def durability(self, value): - assert isinstance(value, QoSDurabilityPolicy) or isinstance(value, int) - self._durability = QoSDurabilityPolicy(value) - - @property - def depth(self): - """ - Get field 'depth'. - - :returns: depth attribute - :rtype: int - """ - return self._depth - - @depth.setter - def depth(self, value): - assert isinstance(value, int) - self._depth = value - - @property - def lifespan(self): - """ - Get field 'lifespan'. - - :returns: lifespan attribute - :rtype: Duration - """ - return self._lifespan - - @lifespan.setter - def lifespan(self, value): - assert isinstance(value, Duration) - self._lifespan = value - - @property - def deadline(self): - """ - Get field 'deadline'. - - :returns: deadline attribute. - :rtype: Duration - """ - return self._deadline - - @deadline.setter - def deadline(self, value): - assert isinstance(value, Duration) - self._deadline = value - - @property - def liveliness(self): - """ - Get field 'liveliness'. - - :returns: liveliness attribute - :rtype: QoSLivelinessPolicy - """ - return self._liveliness - - @liveliness.setter - def liveliness(self, value): - assert isinstance(value, (QoSLivelinessPolicy, int)) - self._liveliness = QoSLivelinessPolicy(value) - - @property - def liveliness_lease_duration(self): - """ - Get field 'liveliness_lease_duration'. - - :returns: liveliness_lease_duration attribute. - :rtype: Duration - """ - return self._liveliness_lease_duration - - @liveliness_lease_duration.setter - def liveliness_lease_duration(self, value): - assert isinstance(value, Duration) - self._liveliness_lease_duration = value - - @property - def avoid_ros_namespace_conventions(self): - """ - Get field 'avoid_ros_namespace_conventions'. - - :returns: avoid_ros_namespace_conventions attribute - :rtype: bool - """ - return self._avoid_ros_namespace_conventions - - @avoid_ros_namespace_conventions.setter - def avoid_ros_namespace_conventions(self, value): - assert isinstance(value, bool) - self._avoid_ros_namespace_conventions = value - - def get_c_qos_profile(self): - return _rclpy.rmw_qos_profile_t( - self.history, - self.depth, - self.reliability, - self.durability, - self.lifespan.get_c_duration(), - self.deadline.get_c_duration(), - self.liveliness, - self.liveliness_lease_duration.get_c_duration(), - self.avoid_ros_namespace_conventions, - ) - - def __eq__(self, other): - if not isinstance(other, QoSProfile): - return False - return all( - self.__getattribute__(slot) == other.__getattribute__(slot) - for slot in self.__slots__) - - def __str__(self): - return f'{type(self).__name__}(%s)' % ( - ', '.join(f'{slot[1:]}=%s' % getattr(self, slot) for slot in self.__slots__) - ) - - -class QoSPolicyEnum(IntEnum): - """ - Base for QoS Policy enumerations. - - Provides helper function to filter keys for utilities. - """ - - @classmethod - def short_keys(cls): - """Return a list of shortened typing-friendly enum values.""" - return [k.lower() for k in cls.__members__.keys() if not k.startswith('RMW')] - - @classmethod - def get_from_short_key(cls, name): - """Retrieve a policy type from a short name, case-insensitive.""" - return cls[name.upper()].value - - @property - def short_key(self): - for k, v in self.__class__.__members__.items(): - if k.startswith('RMW'): - continue - if self.value == v: - return k.lower() - raise AttributeError( - 'failed to find value %s in %s' % - (self.value, self.__class__.__name__)) - - -class _DeprecatedPolicyValueAlias: - """Helper to deprecate a policy value.""" - - def __init__(self, replacement_name, deprecated_name): - self.replacement_name = replacement_name - self.deprecated_name = deprecated_name - - def __get__(self, obj, policy_cls): - warnings.warn( - f'{policy_cls.__name__}.{self.deprecated_name} is deprecated. ' - f'Use {policy_cls.__name__}.{self.replacement_name} instead.' - ) - return policy_cls[self.replacement_name] - - -def _deprecated_policy_value_aliases(pairs): - def decorator(policy_cls): - for deprecated_name, replacement_name in pairs: - setattr( - policy_cls, - deprecated_name, - _DeprecatedPolicyValueAlias(replacement_name, deprecated_name) - ) - return policy_cls - return decorator - - -@_deprecated_policy_value_aliases(( - ('RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT', 'SYSTEM_DEFAULT'), - ('RMW_QOS_POLICY_HISTORY_KEEP_LAST', 'KEEP_LAST'), - ('RMW_QOS_POLICY_HISTORY_KEEP_ALL', 'KEEP_ALL'), - ('RMW_QOS_POLICY_HISTORY_UNKNOWN', 'UNKNOWN'), -)) -class HistoryPolicy(QoSPolicyEnum): - """ - Enum for QoS History settings. - - This enum matches the one defined in rmw/types.h - """ - - SYSTEM_DEFAULT = 0 - KEEP_LAST = 1 - KEEP_ALL = 2 - UNKNOWN = 3 - - -# Alias with the old name, for retrocompatibility -QoSHistoryPolicy = HistoryPolicy - - -@_deprecated_policy_value_aliases(( - ('RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT', 'SYSTEM_DEFAULT'), - ('RMW_QOS_POLICY_RELIABILITY_RELIABLE', 'RELIABLE'), - ('RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT', 'BEST_EFFORT'), - ('RMW_QOS_POLICY_RELIABILITY_UNKNOWN', 'UNKNOWN'), -)) -class ReliabilityPolicy(QoSPolicyEnum): - """ - Enum for QoS Reliability settings. - - This enum matches the one defined in rmw/types.h - """ - - SYSTEM_DEFAULT = 0 - RELIABLE = 1 - BEST_EFFORT = 2 - UNKNOWN = 3 - - -# Alias with the old name, for retrocompatibility -QoSReliabilityPolicy = ReliabilityPolicy - - -@_deprecated_policy_value_aliases(( - ('RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT', 'SYSTEM_DEFAULT'), - ('RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL', 'TRANSIENT_LOCAL'), - ('RMW_QOS_POLICY_DURABILITY_VOLATILE', 'VOLATILE'), - ('RMW_QOS_POLICY_DURABILITY_UNKNOWN', 'UNKNOWN'), -)) -class DurabilityPolicy(QoSPolicyEnum): - """ - Enum for QoS Durability settings. - - This enum matches the one defined in rmw/types.h - """ - - SYSTEM_DEFAULT = 0 - TRANSIENT_LOCAL = 1 - VOLATILE = 2 - UNKNOWN = 3 - - -# Alias with the old name, for retrocompatibility -QoSDurabilityPolicy = DurabilityPolicy - - -@_deprecated_policy_value_aliases(( - ('RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT', 'SYSTEM_DEFAULT'), - ('RMW_QOS_POLICY_LIVELINESS_AUTOMATIC', 'AUTOMATIC'), - ('RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC', 'MANUAL_BY_TOPIC'), - ('RMW_QOS_POLICY_DURABILITY_UNKNOWN', 'UNKNOWN'), -)) -class LivelinessPolicy(QoSPolicyEnum): - """ - Enum for QoS Liveliness settings. - - This enum matches the one defined in rmw/types.h - """ - - SYSTEM_DEFAULT = 0 - AUTOMATIC = 1 - MANUAL_BY_TOPIC = 3 - UNKNOWN = 4 - - -# Alias with the old name, for retrocompatibility -QoSLivelinessPolicy = LivelinessPolicy - -# The details of the following profiles can be found at -# 1. ROS QoS principles: -# https://design.ros2.org/articles/qos.html -# 2. ros2/rmw : rmw/include/rmw/qos_profiles.h - -#: Used for initialization. Should not be used as the actual QoS profile. -qos_profile_unknown = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_unknown').to_dict()) -#: Uses the default QoS settings defined in the DDS vendor tool -qos_profile_system_default = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_system_default').to_dict()) -#: For sensor data, using best effort reliability and small -#: queue depth -qos_profile_sensor_data = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_sensor_data').to_dict()) -#: For services, using reliable reliability and volatile durability -qos_profile_services_default = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_services_default').to_dict()) -#: For parameter communication. Similar to service QoS profile but with larger -#: queue depth so that requests do not get lost. -qos_profile_parameters = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_parameters').to_dict()) -#: For parameter change events. Currently same as the QoS profile for -#: parameters. -qos_profile_parameter_events = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( - 'qos_profile_parameter_events').to_dict()) - -# Separate rcl_action profile defined at -# ros2/rcl : rcl/rcl_action/include/rcl_action/default_qos.h -# -#: For actions, using reliable reliability, transient-local durability. -qos_profile_action_status_default = QoSProfile(**_rclpy.rclpy_action_get_rmw_qos_profile( - 'rcl_action_qos_profile_status_default')) - - -class QoSPresetProfiles(Enum): - UNKNOWN = qos_profile_unknown - 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): - """Return a list of shortened typing-friendly enum values.""" - return [k.lower() for k in cls.__members__.keys() if not k.startswith('RMW')] - - @classmethod - def get_from_short_key(cls, name): - """Retrieve a policy type from a short name, case-insensitive.""" - return cls[name.upper()].value - - -QoSCompatibility = _rclpy.QoSCompatibility - - -def qos_check_compatible(publisher_qos: QoSProfile, subscription_qos: QoSProfile): - """ - Check if two QoS profiles are compatible. - - Two QoS profiles are compatible if a publisher and subscription - using the QoS policies can communicate with each other. - - If any policies have value "system default" or "unknown" then it is possible that - compatibility cannot be determined. - In this case, the value QoSCompatibility.WARNING is set as part of - the returned structure. - """ - result = _rclpy.rclpy_qos_check_compatible( - publisher_qos.get_c_qos_profile(), - subscription_qos.get_c_qos_profile() - ) - compatibility = QoSCompatibility( - result.compatibility - ) - reason = result.reason - return compatibility, reason diff --git a/src/rclpy/qos_event.py b/src/rclpy/qos_event.py deleted file mode 100644 index 4969816..0000000 --- a/src/rclpy/qos_event.py +++ /dev/null @@ -1,291 +0,0 @@ -# Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import IntEnum -from typing import Callable -from typing import List -from typing import Optional - -import rclpy -from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.logging import get_logger -from rclpy.qos import qos_policy_name_from_kind -from rclpy.waitable import NumberOfEntities -from rclpy.waitable import Waitable - - -QoSPublisherEventType = _rclpy.rcl_publisher_event_type_t -QoSSubscriptionEventType = _rclpy.rcl_subscription_event_type_t - - -# Payload type for Subscription Deadline callback. -QoSRequestedDeadlineMissedInfo = _rclpy.rmw_requested_deadline_missed_status_t - -# Payload type for Subscription Liveliness callback. -QoSLivelinessChangedInfo = _rclpy.rmw_liveliness_changed_status_t - -# Payload type for Subscription Message Lost callback. -QoSMessageLostInfo = _rclpy.rmw_message_lost_status_t - -# Payload type for Subscription Incompatible QoS callback. -QoSRequestedIncompatibleQoSInfo = _rclpy.rmw_requested_qos_incompatible_event_status_t - -# Payload type for Publisher Deadline callback. -QoSOfferedDeadlineMissedInfo = _rclpy.rmw_offered_deadline_missed_status_t - -# Payload type for Publisher Liveliness callback. -QoSLivelinessLostInfo = _rclpy.rmw_liveliness_lost_status_t - -""" -Payload type for Publisher Incompatible QoS callback. - -Mirrors rmw_offered_incompatible_qos_status_t from rmw/types.h -""" -QoSOfferedIncompatibleQoSInfo = QoSRequestedIncompatibleQoSInfo - - -"""Raised when registering a callback for an event type that is not supported.""" -UnsupportedEventTypeError = _rclpy.UnsupportedEventTypeError - - -class QoSEventHandler(Waitable): - """Waitable type to handle QoS events.""" - - def __init__( - self, - *, - callback_group: CallbackGroup, - callback: Callable, - event_type: IntEnum, - parent_impl, - ): - # Waitable init adds self to callback_group - super().__init__(callback_group) - self.event_type = event_type - self.callback = callback - - with parent_impl: - self.__event = _rclpy.QoSEvent(parent_impl, event_type) - - self._ready_to_take_data = False - self._event_index = None - - # Start Waitable API - 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 wait_set.is_ready('event', self._event_index): - self._ready_to_take_data = True - return self._ready_to_take_data - - def take_data(self): - """Take stuff from lower level so the wait set doesn't immediately wake again.""" - if self._ready_to_take_data: - self._ready_to_take_data = False - with self.__event: - return self.__event.take_event() - return None - - async def execute(self, taken_data): - """Execute work after data has been taken from a ready wait set.""" - if not taken_data: - return - await rclpy.executors.await_or_execute(self.callback, taken_data) - - def get_num_entities(self): - """Return number of each type of entity used.""" - return NumberOfEntities(num_events=1) - - def add_to_wait_set(self, wait_set): - """Add entites to wait set.""" - with self.__event: - self._event_index = wait_set.add_event(self.__event) - - def __enter__(self): - """Mark event as in-use to prevent destruction while waiting on it.""" - self.__event.__enter__() - - def __exit__(self, t, v, tb): - """Mark event as not-in-use to allow destruction after waiting on it.""" - self.__event.__exit__(t, v, tb) - - def destroy(self): - self.__event.destroy_when_not_in_use() - - -class SubscriptionEventCallbacks: - """Container to provide middleware event callbacks for a Subscription.""" - - def __init__( - self, - *, - deadline: Optional[Callable[[QoSRequestedDeadlineMissedInfo], None]] = None, - incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None, - liveliness: Optional[Callable[[QoSLivelinessChangedInfo], None]] = None, - message_lost: Optional[Callable[[QoSMessageLostInfo], None]] = None, - use_default_callbacks: bool = True, - ) -> None: - """ - Create a SubscriptionEventCallbacks container. - - :param deadline: A user-defined callback that is called when a topic misses our - requested Deadline. - :param incompatible_qos: A user-defined callback that is called when a Publisher - with incompatible QoS policies is discovered on subscribed topic. - :param liveliness: A user-defined callback that is called when the Liveliness of - a Publisher on subscribed topic changes. - :param message_lost: A user-defined callback that is called when a messages is lost. - :param use_default_callbacks: Whether or not to use default callbacks when the user - doesn't supply one - """ - self.deadline = deadline - self.incompatible_qos = incompatible_qos - self.liveliness = liveliness - self.message_lost = message_lost - self.use_default_callbacks = use_default_callbacks - - def create_event_handlers( - self, callback_group: CallbackGroup, subscription: _rclpy.Subscription, topic_name: str, - ) -> List[QoSEventHandler]: - with subscription: - logger = get_logger(subscription.get_logger_name()) - - event_handlers = [] - if self.deadline: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.deadline, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - parent_impl=subscription)) - - if self.incompatible_qos: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.incompatible_qos, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, - parent_impl=subscription)) - elif self.use_default_callbacks: - # Register default callback when not specified - try: - def _default_incompatible_qos_callback(event): - policy_name = qos_policy_name_from_kind(event.last_policy_kind) - logger.warn( - "New publisher discovered on topic '{}', offering incompatible QoS. " - 'No messages will be received from it. ' - 'Last incompatible policy: {}'.format(topic_name, policy_name)) - - event_type = QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=_default_incompatible_qos_callback, - event_type=event_type, - parent_impl=subscription)) - - except UnsupportedEventTypeError: - pass - - if self.liveliness: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.liveliness, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - parent_impl=subscription)) - - if self.message_lost: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.message_lost, - event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_MESSAGE_LOST, - parent_impl=subscription)) - - return event_handlers - - -class PublisherEventCallbacks: - """Container to provide middleware event callbacks for a Publisher.""" - - def __init__( - self, - *, - deadline: Optional[Callable[[QoSOfferedDeadlineMissedInfo], None]] = None, - liveliness: Optional[Callable[[QoSLivelinessLostInfo], None]] = None, - incompatible_qos: Optional[Callable[[QoSRequestedIncompatibleQoSInfo], None]] = None, - use_default_callbacks: bool = True, - ) -> None: - """ - Create and return a PublisherEventCallbacks container. - - :param deadline: A user-defined callback that is called when the Publisher misses - its offered Deadline. - :param liveliness: A user-defined callback that is called when this Publisher - fails to signal its Liveliness and is reported as not-alive. - :param incompatible_qos: A user-defined callback that is called when a Subscription - with incompatible QoS policies is discovered on subscribed topic. - :param use_default_callbacks: Whether or not to use default callbacks when the user - doesn't supply one - """ - self.deadline = deadline - self.liveliness = liveliness - self.incompatible_qos = incompatible_qos - self.use_default_callbacks = use_default_callbacks - - def create_event_handlers( - self, callback_group: CallbackGroup, publisher: _rclpy.Publisher, topic_name: str, - ) -> List[QoSEventHandler]: - with publisher: - logger = get_logger(publisher.get_logger_name()) - - event_handlers = [] - if self.deadline: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.deadline, - event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, - parent_impl=publisher)) - - if self.liveliness: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.liveliness, - event_type=QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, - parent_impl=publisher)) - - if self.incompatible_qos: - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=self.incompatible_qos, - event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_impl=publisher)) - elif self.use_default_callbacks: - # Register default callback when not specified - try: - def _default_incompatible_qos_callback(event): - policy_name = qos_policy_name_from_kind(event.last_policy_kind) - logger.warn( - "New subscription discovered on topic '{}', requesting incompatible QoS. " - 'No messages will be sent to it. ' - 'Last incompatible policy: {}'.format(topic_name, policy_name)) - - event_handlers.append(QoSEventHandler( - callback_group=callback_group, - callback=_default_incompatible_qos_callback, - event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_impl=publisher)) - - except UnsupportedEventTypeError: - pass - - return event_handlers diff --git a/src/rclpy/qos_overriding_options.py b/src/rclpy/qos_overriding_options.py deleted file mode 100644 index 316b594..0000000 --- a/src/rclpy/qos_overriding_options.py +++ /dev/null @@ -1,190 +0,0 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Callable -from typing import Iterable -from typing import Optional -from typing import Text -from typing import Type -from typing import TYPE_CHECKING -from typing import Union - -from rcl_interfaces.msg import ParameterDescriptor -from rcl_interfaces.msg import SetParametersResult - -import rclpy -from rclpy.duration import Duration -from rclpy.exceptions import ParameterAlreadyDeclaredException -from rclpy.parameter import Parameter -from rclpy.publisher import Publisher -from rclpy.qos import QoSPolicyKind -from rclpy.qos import QoSProfile -from rclpy.subscription import Subscription - -if TYPE_CHECKING: - from rclpy.node import Node - - -class InvalidQosOverridesError(Exception): - pass - - -# Return type of qos validation callbacks -QosCallbackResult = SetParametersResult -# Qos callback type annotation -QosCallbackType = Callable[[QoSProfile], QosCallbackResult] - - -class QoSOverridingOptions: - """Options to customize QoS parameter overrides.""" - - def __init__( - self, - policy_kinds: Iterable[QoSPolicyKind], - *, - callback: Optional[QosCallbackType] = None, - entity_id: Optional[Text] = None - ): - """ - Construct a QoSOverridingOptions object. - - :param policy_kinds: QoS kinds that will have a declared parameter. - :param callback: Callback that will be used to validate the QoS profile - after the paramter overrides get applied. - :param entity_id: Optional identifier, to disambiguate in the case that different QoS - policies for the same topic are desired. - """ - self._policy_kinds = policy_kinds - self._callback = callback - self._entity_id = entity_id - - @property - def policy_kinds(self) -> Iterable[QoSPolicyKind]: - """Get QoS policy kinds that will have a parameter override.""" - return self._policy_kinds - - @property - def callback(self) -> Optional[QosCallbackType]: - """Get the validation callback.""" - return self._callback - - @property - def entity_id(self) -> Optional[Text]: - """Get the optional entity ID.""" - return self._entity_id - - @classmethod - def with_default_policies( - cls, *, - callback: Optional[QosCallbackType] = None, - entity_id: Optional[Text] = None - ) -> 'QoSOverridingOptions': - return cls( - policy_kinds=(QoSPolicyKind.HISTORY, QoSPolicyKind.DEPTH, QoSPolicyKind.RELIABILITY), - callback=callback, - entity_id=entity_id, - ) - - -def _declare_qos_parameters( - entity_type: Union[Type[Publisher], Type[Subscription]], - node: 'Node', - topic_name: Text, - qos: QoSProfile, - options: QoSOverridingOptions -) -> QoSProfile: - """ - Declare QoS parameters for a Publisher or a Subscription. - - :param entity_type: Either `rclpy.node.Publisher` or `rclpy.node.Subscription`. - :param node: Node used to declare the parameters. - :param topic_name: Topic name of the entity being created. - :param qos: Default QoS settings of the entity being created, that will be overridden - with the user provided QoS parameter overrides. - :param options: Options that indicates which parameters are going to be declared. - """ - if not issubclass(entity_type, (Publisher, Subscription)): - raise TypeError('Argument `entity_type` should be a subclass of Publisher or Subscription') - entity_type_str = 'publisher' if issubclass(entity_type, Publisher) else 'subscription' - id_suffix = '' if options.entity_id is None else f'_{options.entity_id}' - name = f'qos_overrides.{topic_name}.{entity_type_str}{id_suffix}.' '{}' - description = '{}' f' for {entity_type_str} `{topic_name}` with id `{options.entity_id}`' - allowed_policies = _get_allowed_policies(entity_type) - for policy in options.policy_kinds: - if policy not in allowed_policies: - continue - policy_name = policy.name.lower() - descriptor = ParameterDescriptor() - descriptor.description = description.format(policy_name) - descriptor.read_only = True - try: - param = node.declare_parameter( - name.format(policy_name), - _get_qos_policy_parameter(qos, policy), - descriptor) - except ParameterAlreadyDeclaredException: - param = node.get_parameter(name.format(policy_name)) - _override_qos_policy_with_param(qos, policy, param) - if options.callback is not None: - result = options.callback(qos) - if not result.successful: - raise InvalidQosOverridesError( - f"{description.format('Provided QoS overrides')}, are not valid: {result.reason}") - - -def _get_allowed_policies(entity_type: Union[Type[Publisher], Type[Subscription]]): - allowed_policies = list(QoSPolicyKind.__members__.values()) - if issubclass(entity_type, Subscription): - allowed_policies.remove(QoSPolicyKind.LIFESPAN) - return allowed_policies - - -def _get_qos_policy_parameter(qos: QoSProfile, policy: QoSPolicyKind) -> Union[str, int, bool]: - value = getattr(qos, policy.name.lower()) - if policy in ( - QoSPolicyKind.LIVELINESS, QoSPolicyKind.RELIABILITY, - QoSPolicyKind.HISTORY, QoSPolicyKind.DURABILITY - ): - value = value.name.lower() - if value == 'unknown': - raise ValueError('User provided QoS profile is invalid') - if policy in ( - QoSPolicyKind.LIFESPAN, QoSPolicyKind.DEADLINE, QoSPolicyKind.LIVELINESS_LEASE_DURATION - ): - value = value.nanoseconds - return value - - -def _override_qos_policy_with_param(qos: QoSProfile, policy: QoSPolicyKind, param: Parameter): - value = param.value - policy_name = policy.name.lower() - if policy in ( - QoSPolicyKind.LIVELINESS, QoSPolicyKind.RELIABILITY, - QoSPolicyKind.HISTORY, QoSPolicyKind.DURABILITY - ): - def capitalize_first_letter(x): - return x[0].upper() + x[1:] - # e.g. `policy=QosPolicyKind.LIVELINESS` -> `policy_enum_class=rclpy.qos.LivelinessPolicy` - policy_enum_class = getattr( - rclpy.qos, f'{capitalize_first_letter(policy_name)}Policy') - try: - value = policy_enum_class[value.upper()] - except KeyError: - raise RuntimeError( - f'Unexpected QoS override for policy `{policy.name.lower()}`: `{value}`') - if policy in ( - QoSPolicyKind.LIFESPAN, QoSPolicyKind.DEADLINE, QoSPolicyKind.LIVELINESS_LEASE_DURATION - ): - value = Duration(nanoseconds=value) - setattr(qos, policy.name.lower(), value) diff --git a/src/rclpy/serialization.py b/src/rclpy/serialization.py deleted file mode 100644 index 94c899f..0000000 --- a/src/rclpy/serialization.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.type_support import check_for_type_support - - -def serialize_message(message) -> bytes: - """ - Serialize a ROS message. - - :param message: The ROS message to serialize. - :return: The serialized bytes. - """ - message_type = type(message) - # this line imports the typesupport for the message module if not already done - check_for_type_support(message_type) - return _rclpy.rclpy_serialize(message, message_type) - - -def deserialize_message(serialized_message: bytes, message_type): - """ - Deserialize a ROS message. - - :param serialized_message: The ROS message to deserialized. - :param message_type: The type of the serialized ROS message. - :return: The deserialized ROS message. - """ - # this line imports the typesupport for the message module if not already done - check_for_type_support(message_type) - return _rclpy.rclpy_deserialize(serialized_message, message_type) diff --git a/src/rclpy/service.py b/src/rclpy/service.py deleted file mode 100644 index 3fe2787..0000000 --- a/src/rclpy/service.py +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Callable -from typing import TypeVar - -from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import QoSProfile - -# Used for documentation purposes only -SrvType = TypeVar('SrvType') -SrvTypeRequest = TypeVar('SrvTypeRequest') -SrvTypeResponse = TypeVar('SrvTypeResponse') - - -class Service: - def __init__( - self, - service_impl: _rclpy.Service, - srv_type: SrvType, - srv_name: str, - callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], - callback_group: CallbackGroup, - qos_profile: QoSProfile - ) -> None: - """ - Create a container for a ROS service server. - - .. warning:: Users should not create a service server with this constuctor, instead they - should call :meth:`.Node.create_service`. - - :param context: The context associated with the service server. - :param service_impl: :class:`_rclpy.Service` wrapping the underlying ``rcl_service_t`` - object. - :param srv_type: The service type. - :param srv_name: The name of the service. - :param callback_group: The callback group for the service server. If ``None``, then the - nodes default callback group is used. - :param qos_profile: The quality of service profile to apply the service server. - """ - self.__service = service_impl - self.srv_type = srv_type - self.srv_name = srv_name - self.callback = callback - self.callback_group = callback_group - # True when the callback is ready to fire but has not been "taken" by an executor - self._executor_event = False - self.qos_profile = qos_profile - - def send_response(self, response: SrvTypeResponse, header) -> None: - """ - Send a service response. - - :param response: The service response. - :param header: Service header from the original request. - :raises: TypeError if the type of the passed response isn't an instance - of the Response type of the provided service when the service was - constructed. - """ - if not isinstance(response, self.srv_type.Response): - raise TypeError() - with self.handle: - if isinstance(header, _rclpy.rmw_service_info_t): - self.__service.service_send_response(response, header.request_id) - elif isinstance(header, _rclpy.rmw_request_id_t): - self.__service.service_send_response(response, header) - else: - raise TypeError() - - @property - def handle(self): - return self.__service - - def destroy(self): - self.__service.destroy_when_not_in_use() diff --git a/src/rclpy/signals.py b/src/rclpy/signals.py deleted file mode 100644 index 5c61625..0000000 --- a/src/rclpy/signals.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidHandle -from rclpy.guard_condition import GuardCondition -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -# re-export SignalHandlerOptions enum -SignalHandlerOptions = _rclpy.SignalHandlerOptions - - -def install_signal_handlers(options: SignalHandlerOptions = SignalHandlerOptions.ALL): - """ - Install rclpy signal handlers. - - :param options: Indicate if to install sigint, sigterm, both or no signal handler. - """ - return _rclpy.install_signal_handlers(options) - - -def get_current_signal_handlers_options(): - """ - Get current signal handler options. - - :return: rclpy.signals.SignalHandlerOptions instance. - """ - return _rclpy.get_current_signal_handlers_options() - - -def uninstall_signal_handlers(): - """Uninstall the rclpy signal handlers.""" - _rclpy.uninstall_signal_handlers() - - -class SignalHandlerGuardCondition(GuardCondition): - - def __init__(self, context=None): - super().__init__(callback=None, callback_group=None, context=context) - with self.handle: - _rclpy.register_sigint_guard_condition(self.handle) - - def __del__(self): - try: - self.destroy() - except InvalidHandle: - # already destroyed - pass - except ValueError: - # Guard condition was not registered - pass - - def destroy(self): - with self.handle: - _rclpy.unregister_sigint_guard_condition(self.handle) - super().destroy() diff --git a/src/rclpy/subscription.py b/src/rclpy/subscription.py deleted file mode 100644 index ed16d5e..0000000 --- a/src/rclpy/subscription.py +++ /dev/null @@ -1,85 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Callable -from typing import TypeVar - -from rclpy.callback_groups import CallbackGroup -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.qos import QoSProfile -from rclpy.qos_event import QoSEventHandler -from rclpy.qos_event import SubscriptionEventCallbacks - - -# For documentation only -MsgType = TypeVar('MsgType') - - -class Subscription: - - def __init__( - self, - subscription_impl: _rclpy.Subscription, - msg_type: MsgType, - topic: str, - callback: Callable, - callback_group: CallbackGroup, - qos_profile: QoSProfile, - raw: bool, - event_callbacks: SubscriptionEventCallbacks, - ) -> None: - """ - Create a container for a ROS subscription. - - .. warning:: Users should not create a subscription with this constructor, instead they - should call :meth:`.Node.create_subscription`. - - :param subscription_impl: :class:`Subscription` wrapping the underlying - ``rcl_subscription_t`` object. - :param msg_type: The type of ROS messages the subscription will subscribe to. - :param topic: The name of the topic the subscription will subscribe to. - :param callback: A user-defined callback function that is called when a message is - received by the subscription. - :param callback_group: The callback group for the subscription. If ``None``, then the - nodes default callback group is used. - :param qos_profile: The quality of service profile to apply to the subscription. - :param raw: If ``True``, then received messages will be stored in raw binary - representation. - """ - self.__subscription = subscription_impl - self.msg_type = msg_type - self.topic = topic - self.callback = callback - self.callback_group = callback_group - # True when the callback is ready to fire but has not been "taken" by an executor - self._executor_event = False - self.qos_profile = qos_profile - self.raw = raw - - self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, subscription_impl, topic) - - @property - def handle(self): - return self.__subscription - - def destroy(self): - for handler in self.event_handlers: - handler.destroy() - self.handle.destroy_when_not_in_use() - - @property - def topic_name(self): - with self.handle: - return self.__subscription.get_topic_name() diff --git a/src/rclpy/task.py b/src/rclpy/task.py deleted file mode 100644 index 2ed1d6c..0000000 --- a/src/rclpy/task.py +++ /dev/null @@ -1,273 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import inspect -import sys -import threading -import warnings -import weakref - - -def _fake_weakref(): - """Return None when called to simulate a weak reference that has been garbage collected.""" - return None - - -class Future: - """Represent the outcome of a task in the future.""" - - def __init__(self, *, executor=None): - # true if the task is done or cancelled - self._done = False - # true if the task is cancelled - self._cancelled = False - # the final return value of the handler - self._result = None - # An exception raised by the handler when called - self._exception = None - self._exception_fetched = False - # callbacks to be scheduled after this task completes - self._callbacks = [] - # Lock for threadsafety - self._lock = threading.Lock() - # An executor to use when scheduling done callbacks - self._executor = None - self._set_executor(executor) - - def __del__(self): - if self._exception is not None and not self._exception_fetched: - print( - 'The following exception was never retrieved: ' + str(self._exception), - file=sys.stderr) - - def __await__(self): - # Yield if the task is not finished - while not self._done: - yield - return self.result() - - def cancel(self): - """Request cancellation of the running task if it is not done already.""" - with self._lock: - if not self._done: - self._cancelled = True - self._schedule_or_invoke_done_callbacks() - - def cancelled(self): - """ - Indicate if the task has been cancelled. - - :return: True if the task was cancelled - :rtype: bool - """ - return self._cancelled - - def done(self): - """ - Indicate if the task has finished executing. - - :return: True if the task is finished or raised while it was executing - :rtype: bool - """ - return self._done - - def result(self): - """ - Get the result of a done task. - - :raises: Exception if one was set during the task. - - :return: The result set by the task, or None if no result was set. - """ - if self._exception: - raise self.exception() - return self._result - - def exception(self): - """ - Get an exception raised by a done task. - - :return: The exception raised by the task - """ - self._exception_fetched = True - return self._exception - - def set_result(self, result): - """ - Set the result returned by a task. - - :param result: The output of a long running task. - """ - with self._lock: - self._result = result - self._done = True - self._cancelled = False - self._schedule_or_invoke_done_callbacks() - - def set_exception(self, exception): - """ - Set the exception raised by the task. - - :param result: The output of a long running task. - """ - with self._lock: - self._exception = exception - self._exception_fetched = False - self._done = True - self._cancelled = False - self._schedule_or_invoke_done_callbacks() - - def _schedule_or_invoke_done_callbacks(self): - """ - Schedule done callbacks on the executor if possible, else run them directly. - - This function assumes self._lock is not held. - """ - with self._lock: - executor = self._executor() - callbacks = self._callbacks - self._callbacks = [] - - if executor is not None: - # Have the executor take care of the callbacks - for callback in callbacks: - executor.create_task(callback, self) - else: - # No executor, call right away - for callback in callbacks: - try: - callback(self) - except Exception as e: - # Don't let exceptions be raised because there may be more callbacks to call - warnings.warn('Unhandled exception in done callback: {}'.format(e)) - - def _set_executor(self, executor): - """Set the executor this future is associated with.""" - with self._lock: - if executor is None: - self._executor = _fake_weakref - else: - self._executor = weakref.ref(executor) - - def add_done_callback(self, callback): - """ - Add a callback to be executed when the task is done. - - Callbacks should not raise exceptions. - - The callback may be called immediately by this method if the future is already done. - If this happens and the callback raises, the exception will be raised by this method. - - :param callback: a callback taking the future as an agrument to be run when completed - """ - invoke = False - with self._lock: - if self._done: - executor = self._executor() - if executor is not None: - executor.create_task(callback, self) - else: - invoke = True - else: - self._callbacks.append(callback) - - # Invoke when not holding self._lock - if invoke: - callback(self) - - -class Task(Future): - """ - Execute a function or coroutine. - - This executes either a normal function or a coroutine to completion. On completion it creates - tasks for any 'done' callbacks. - - This class should only be instantiated by :class:`rclpy.executors.Executor`. - """ - - def __init__(self, handler, args=None, kwargs=None, executor=None): - super().__init__(executor=executor) - # _handler is either a normal function or a coroutine - self._handler = handler - # Arguments passed into the function - if args is None: - args = [] - self._args = args - if kwargs is None: - kwargs = {} - self._kwargs = kwargs - if inspect.iscoroutinefunction(handler): - self._handler = handler(*args, **kwargs) - self._args = None - self._kwargs = None - # True while the task is being executed - self._executing = False - # Lock acquired to prevent task from executing in parallel with itself - self._task_lock = threading.Lock() - - def __call__(self): - """ - Run or resume a task. - - This attempts to execute a handler. If the handler is a coroutine it will attempt to - await it. If there are done callbacks it will schedule them with the executor. - - The return value of the handler is stored as the task result. - """ - if self._done or self._executing or not self._task_lock.acquire(blocking=False): - return - try: - if self._done: - return - self._executing = True - - if inspect.iscoroutine(self._handler): - # Execute a coroutine - try: - self._handler.send(None) - except StopIteration as e: - # The coroutine finished; store the result - self._handler.close() - self.set_result(e.value) - self._complete_task() - except Exception as e: - self.set_exception(e) - self._complete_task() - else: - # Execute a normal function - try: - self.set_result(self._handler(*self._args, **self._kwargs)) - except Exception as e: - self.set_exception(e) - self._complete_task() - - self._executing = False - finally: - self._task_lock.release() - - def _complete_task(self): - """Cleanup after task finished.""" - self._handler = None - self._args = None - self._kwargs = None - - def executing(self): - """ - Check if the task is currently being executed. - - :return: True if the task is currently executing. - :rtype: bool - """ - return self._executing diff --git a/src/rclpy/time.py b/src/rclpy/time.py deleted file mode 100644 index ee4934d..0000000 --- a/src/rclpy/time.py +++ /dev/null @@ -1,147 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import builtin_interfaces.msg -from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -CONVERSION_CONSTANT = 10 ** 9 - - -class Time: - - def __init__( - self, *, - seconds=0, nanoseconds=0, - clock_type: _rclpy.ClockType = _rclpy.ClockType.SYSTEM_TIME): - if not isinstance(clock_type, _rclpy.ClockType): - raise TypeError('Clock type must be a ClockType enum') - if seconds < 0: - raise ValueError('Seconds value must not be negative') - if nanoseconds < 0: - raise ValueError('Nanoseconds value must not be negative') - total_nanoseconds = int(seconds * CONVERSION_CONSTANT) - total_nanoseconds += int(nanoseconds) - if total_nanoseconds >= 2**63: - # pybind11 would raise TypeError, but we want OverflowError - raise OverflowError( - 'Total nanoseconds value is too large to store in C time point.') - self._time_handle = _rclpy.rcl_time_point_t(total_nanoseconds, clock_type) - - @property - def nanoseconds(self): - return self._time_handle.nanoseconds - - def seconds_nanoseconds(self): - """ - Get time as separate seconds and nanoseconds components. - - :returns: 2-tuple seconds and nanoseconds - :rtype: tuple(int, int) - """ - nanoseconds = self.nanoseconds - return (nanoseconds // CONVERSION_CONSTANT, nanoseconds % CONVERSION_CONSTANT) - - @property - def clock_type(self): - return self._time_handle.clock_type - - def __repr__(self): - return 'Time(nanoseconds={0}, clock_type={1})'.format( - self.nanoseconds, self.clock_type.name) - - def __add__(self, other): - if isinstance(other, Duration): - try: - return Time( - nanoseconds=(self.nanoseconds + other.nanoseconds), - clock_type=self.clock_type) - except OverflowError as e: - raise OverflowError('Addition leads to overflow in C storage.') from e - else: - return NotImplemented - - def __radd__(self, other): - return self.__add__(other) - - def __sub__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't subtract times with different clock types") - try: - return Duration(nanoseconds=(self.nanoseconds - other.nanoseconds)) - except ValueError as e: - raise ValueError('Subtraction leads to negative duration.') from e - if isinstance(other, Duration): - try: - return Time( - nanoseconds=(self.nanoseconds - other.nanoseconds), - clock_type=self.clock_type) - except ValueError as e: - raise ValueError('Subtraction leads to negative time.') from e - else: - return NotImplemented - - def __eq__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't compare times with different clock types") - return self.nanoseconds == other.nanoseconds - # Raise instead of returning NotImplemented to prevent comparison with invalid types, - # e.g. ints. - # Otherwise `Time(nanoseconds=5) == 5` will return False instead of raising, and this - # could lead to hard-to-find bugs. - raise TypeError("Can't compare time with object of type: ", type(other)) - - def __ne__(self, other): - return not self.__eq__(other) - - def __lt__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't compare times with different clock types") - return self.nanoseconds < other.nanoseconds - return NotImplemented - - def __le__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't compare times with different clock types") - return self.nanoseconds <= other.nanoseconds - return NotImplemented - - def __gt__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't compare times with different clock types") - return self.nanoseconds > other.nanoseconds - return NotImplemented - - def __ge__(self, other): - if isinstance(other, Time): - if self.clock_type != other.clock_type: - raise TypeError("Can't compare times with different clock types") - return self.nanoseconds >= other.nanoseconds - return NotImplemented - - def to_msg(self): - seconds, nanoseconds = self.seconds_nanoseconds() - return builtin_interfaces.msg.Time(sec=seconds, nanosec=nanoseconds) - - @classmethod - def from_msg(cls, msg, clock_type: _rclpy.ClockType = _rclpy.ClockType.ROS_TIME): - if not isinstance(msg, builtin_interfaces.msg.Time): - raise TypeError('Must pass a builtin_interfaces.msg.Time object') - return cls(seconds=msg.sec, nanoseconds=msg.nanosec, clock_type=clock_type) diff --git a/src/rclpy/time_source.py b/src/rclpy/time_source.py deleted file mode 100644 index ec052e7..0000000 --- a/src/rclpy/time_source.py +++ /dev/null @@ -1,143 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import weakref - -from rcl_interfaces.msg import SetParametersResult -from rclpy.clock import ClockType -from rclpy.clock import ROSClock -from rclpy.parameter import Parameter -from rclpy.qos import QoSProfile -from rclpy.qos import ReliabilityPolicy -from rclpy.time import Time -import rosgraph_msgs.msg - -CLOCK_TOPIC = '/clock' -USE_SIM_TIME_NAME = 'use_sim_time' - - -class TimeSource: - - def __init__(self, *, node=None): - self._clock_sub = None - self._node_weak_ref = None - self._associated_clocks = [] - # Zero time is a special value that means time is uninitialzied - self._last_time_set = Time(clock_type=ClockType.ROS_TIME) - self._ros_time_is_active = False - if node is not None: - self.attach_node(node) - - @property - def ros_time_is_active(self): - return self._ros_time_is_active - - @ros_time_is_active.setter - def ros_time_is_active(self, enabled): - if self._ros_time_is_active == enabled: - return - self._ros_time_is_active = enabled - for clock in self._associated_clocks: - clock._set_ros_time_is_active(enabled) - if enabled: - self._subscribe_to_clock_topic() - else: - if self._clock_sub is not None: - node = self._get_node() - if node is not None: - node.destroy_subscription(self._clock_sub) - self._clock_sub = None - - def _subscribe_to_clock_topic(self): - if self._clock_sub is None: - node = self._get_node() - if node is not None: - self._clock_sub = node.create_subscription( - rosgraph_msgs.msg.Clock, - CLOCK_TOPIC, - self.clock_callback, - QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT) - ) - - def attach_node(self, node): - from rclpy.node import Node - if not isinstance(node, Node): - raise TypeError('Node must be of type rclpy.node.Node') - # Remove an existing node. - if self._node_weak_ref is not None: - self.detach_node() - self._node_weak_ref = weakref.ref(node) - - if not node.has_parameter(USE_SIM_TIME_NAME): - node.declare_parameter(USE_SIM_TIME_NAME, False) - - use_sim_time_param = node.get_parameter(USE_SIM_TIME_NAME) - if use_sim_time_param.type_ != Parameter.Type.NOT_SET: - if use_sim_time_param.type_ == Parameter.Type.BOOL: - self.ros_time_is_active = use_sim_time_param.value - else: - node.get_logger().error( - "Invalid type for parameter '{}' {!r} should be bool" - .format(USE_SIM_TIME_NAME, use_sim_time_param.type_)) - else: - node.get_logger().debug( - "'{}' parameter not set, using wall time by default" - .format(USE_SIM_TIME_NAME)) - - node.add_on_set_parameters_callback(self._on_parameter_event) - - def detach_node(self): - # Remove the subscription to the clock topic. - if self._clock_sub is not None: - node = self._get_node() - if node is None: - raise RuntimeError('Unable to destroy previously created clock subscription') - node.destroy_subscription(self._clock_sub) - self._clock_sub = None - self._node_weak_ref = None - - def attach_clock(self, clock): - if not isinstance(clock, ROSClock): - raise ValueError('Only clocks with type ROS_TIME can be attached.') - - clock.set_ros_time_override(self._last_time_set) - clock._set_ros_time_is_active(self.ros_time_is_active) - self._associated_clocks.append(clock) - - def clock_callback(self, msg): - # Cache the last message in case a new clock is attached. - time_from_msg = Time.from_msg(msg.clock) - self._last_time_set = time_from_msg - for clock in self._associated_clocks: - clock.set_ros_time_override(time_from_msg) - - def _on_parameter_event(self, parameter_list): - for parameter in parameter_list: - if parameter.name == USE_SIM_TIME_NAME: - if parameter.type_ == Parameter.Type.BOOL: - self.ros_time_is_active = parameter.value - else: - node = self._get_node() - if node: - node.get_logger().error( - '{} parameter set to something besides a bool' - .format(USE_SIM_TIME_NAME)) - break - - return SetParametersResult(successful=True) - - def _get_node(self): - if self._node_weak_ref is not None: - return self._node_weak_ref() - return None diff --git a/src/rclpy/timer.py b/src/rclpy/timer.py deleted file mode 100644 index 0c3be6d..0000000 --- a/src/rclpy/timer.py +++ /dev/null @@ -1,145 +0,0 @@ -# Copyright 2016 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import threading - -from rclpy.exceptions import InvalidHandle, ROSInterruptException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.utilities import get_default_context - - -class Timer: - - def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): - self._context = get_default_context() if context is None else context - self._clock = clock - with self._clock.handle, self._context.handle: - self.__timer = _rclpy.Timer( - self._clock.handle, self._context.handle, timer_period_ns) - self.timer_period_ns = timer_period_ns - self.callback = callback - self.callback_group = callback_group - # True when the callback is ready to fire but has not been "taken" by an executor - self._executor_event = False - - @property - def handle(self): - return self.__timer - - def destroy(self): - self.__timer.destroy_when_not_in_use() - - @property - def clock(self): - return self._clock - - @property - def timer_period_ns(self): - with self.__timer: - val = self.__timer.get_timer_period() - self.__timer_period_ns = val - return val - - @timer_period_ns.setter - def timer_period_ns(self, value): - val = int(value) - with self.__timer: - self.__timer.change_timer_period(val) - self.__timer_period_ns = val - - def is_ready(self): - with self.__timer: - return self.__timer.is_timer_ready() - - def is_canceled(self): - with self.__timer: - return self.__timer.is_timer_canceled() - - def cancel(self): - with self.__timer: - self.__timer.cancel_timer() - - def reset(self): - with self.__timer: - self.__timer.reset_timer() - - def time_since_last_call(self): - with self.__timer: - return self.__timer.time_since_last_call() - - def time_until_next_call(self): - with self.__timer: - return self.__timer.time_until_next_call() - - -class Rate: - """A utility for sleeping at a fixed rate.""" - - def __init__(self, timer: Timer, *, context): - # Rate is a wrapper around a timer - self._timer = timer - self._is_shutdown = False - self._is_destroyed = False - - # This event is set to wake sleepers - self._event = threading.Event() - self._lock = threading.Lock() - self._num_sleepers = 0 - - # Set event when timer fires - self._timer.callback = self._event.set - - # Set event when ROS is shutdown - context.on_shutdown(self._on_shutdown) - - def _on_shutdown(self): - self._is_shutdown = True - self.destroy() - - def destroy(self): - self._is_destroyed = True - self._event.set() - - def _presleep(self): - if self._is_shutdown: - raise ROSInterruptException() - if self._is_destroyed: - raise RuntimeError('Rate cannot sleep because it has been destroyed') - if not self._timer.handle: - self.destroy() - raise InvalidHandle('Rate cannot sleep because the timer has been destroyed') - with self._lock: - self._num_sleepers += 1 - - def _postsleep(self): - with self._lock: - self._num_sleepers -= 1 - if self._num_sleepers == 0: - self._event.clear() - if self._is_shutdown: - self.destroy() - raise ROSInterruptException() - - def sleep(self): - """ - Block until timer triggers. - - Care should be taken when calling this from a callback. - This may block forever if called in a callback in a SingleThreadedExecutor. - """ - self._presleep() - try: - self._event.wait() - finally: - self._postsleep() diff --git a/src/rclpy/topic_endpoint_info.py b/src/rclpy/topic_endpoint_info.py deleted file mode 100644 index f84d6fd..0000000 --- a/src/rclpy/topic_endpoint_info.py +++ /dev/null @@ -1,180 +0,0 @@ -# Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from enum import IntEnum - -from rclpy.qos import QoSHistoryPolicy, QoSPresetProfiles, QoSProfile - - -class TopicEndpointTypeEnum(IntEnum): - """ - Enum for possible types of topic endpoints. - - This enum matches the one defined in rmw/types.h - """ - - INVALID = 0 - PUBLISHER = 1 - SUBSCRIPTION = 2 - - -class TopicEndpointInfo: - """Information on a topic endpoint.""" - - __slots__ = [ - '_node_name', - '_node_namespace', - '_topic_type', - '_endpoint_type', - '_endpoint_gid', - '_qos_profile' - ] - - def __init__(self, **kwargs): - assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ - 'Invalid arguments passed to constructor: %r' % kwargs.keys() - - self.node_name = kwargs.get('node_name', '') - self.node_namespace = kwargs.get('node_namespace', '') - self.topic_type = kwargs.get('topic_type', '') - self.endpoint_type = kwargs.get('endpoint_type', TopicEndpointTypeEnum.INVALID) - self.endpoint_gid = kwargs.get('endpoint_gid', []) - self.qos_profile = kwargs.get('qos_profile', QoSPresetProfiles.UNKNOWN.value) - - @property - def node_name(self): - """ - Get field 'node_name'. - - :returns: node_name attribute - :rtype: str - """ - return self._node_name - - @node_name.setter - def node_name(self, value): - assert isinstance(value, str) - self._node_name = value - - @property - def node_namespace(self): - """ - Get field 'node_namespace'. - - :returns: node_namespace attribute - :rtype: str - """ - return self._node_namespace - - @node_namespace.setter - def node_namespace(self, value): - assert isinstance(value, str) - self._node_namespace = value - - @property - def topic_type(self): - """ - Get field 'topic_type'. - - :returns: topic_type attribute - :rtype: str - """ - return self._topic_type - - @topic_type.setter - def topic_type(self, value): - assert isinstance(value, str) - self._topic_type = value - - @property - def endpoint_type(self): - """ - Get field 'endpoint_type'. - - :returns: endpoint_type attribute - :rtype: TopicEndpointTypeEnum - """ - return self._endpoint_type - - @endpoint_type.setter - def endpoint_type(self, value): - if isinstance(value, TopicEndpointTypeEnum): - self._endpoint_type = value - elif isinstance(value, int): - self._endpoint_type = TopicEndpointTypeEnum(value) - else: - assert False - - @property - def endpoint_gid(self): - """ - Get field 'endpoint_gid'. - - :returns: endpoint_gid attribute - :rtype: list - """ - return self._endpoint_gid - - @endpoint_gid.setter - def endpoint_gid(self, value): - assert all(isinstance(x, int) for x in value) - self._endpoint_gid = value - - @property - def qos_profile(self): - """ - Get field 'qos_profile'. - - :returns: qos_profile attribute - :rtype: QoSProfile - """ - return self._qos_profile - - @qos_profile.setter - def qos_profile(self, value): - if isinstance(value, QoSProfile): - self._qos_profile = value - elif isinstance(value, dict): - self._qos_profile = QoSProfile(**value) - else: - assert False - - def __eq__(self, other): - if not isinstance(other, TopicEndpointInfo): - return False - return all( - self.__getattribute__(slot) == other.__getattribute__(slot) - for slot in self.__slots__) - - def __str__(self): - gid = '.'.join(format(x, '02x') for x in self.endpoint_gid) - if self.qos_profile.history.value != QoSHistoryPolicy.KEEP_LAST: - history_depth_str = self.qos_profile.history.name - else: - history_depth_str = f'{self.qos_profile.history.name} ({self.qos_profile.depth})' - return '\n'.join([ - f'Node name: {self.node_name}', - f'Node namespace: {self.node_namespace}', - f'Topic type: {self.topic_type}', - f'Endpoint type: {self.endpoint_type.name}', - f'GID: {gid}', - 'QoS profile:', - f' Reliability: {self.qos_profile.reliability.name}', - f' History (Depth): {history_depth_str}', - f' Durability: {self.qos_profile.durability.name}', - f' Lifespan: {self.qos_profile.lifespan}', - f' Deadline: {self.qos_profile.deadline}', - f' Liveliness: {self.qos_profile.liveliness.name}', - f' Liveliness lease duration: {self.qos_profile.liveliness_lease_duration}', - ]) diff --git a/src/rclpy/topic_or_service_is_hidden.py b/src/rclpy/topic_or_service_is_hidden.py deleted file mode 100644 index 3486532..0000000 --- a/src/rclpy/topic_or_service_is_hidden.py +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -HIDDEN_TOPIC_PREFIX = '_' - - -def topic_or_service_is_hidden(name): - """ - Return True if a given topic or service name is hidden, otherwise False. - - A topic or service name is considered hidden if any of the name tokens - begins with an underscore (``_``). - See: - - http://design.ros2.org/articles/topic_and_service_names.html#hidden-topic-or-service-names - - :param name str: topic or service name to test - :returns: True if name is hidden, otherwise False - """ - return any(token for token in name.split('/') if token.startswith(HIDDEN_TOPIC_PREFIX)) diff --git a/src/rclpy/type_support.py b/src/rclpy/type_support.py deleted file mode 100644 index 9deddf5..0000000 --- a/src/rclpy/type_support.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2016-2021 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import NoTypeSupportImportedException - - -def check_for_type_support(msg_or_srv_type): - try: - ts = msg_or_srv_type.__class__._TYPE_SUPPORT - except AttributeError as e: - e.args = ( - e.args[0] + - ' This might be a ROS 1 message type but it should be a ROS 2 message type.' - ' Make sure to source your ROS 2 workspace after your ROS 1 workspace.', - *e.args[1:]) - raise - if ts is None: - msg_or_srv_type.__class__.__import_type_support__() - if msg_or_srv_type.__class__._TYPE_SUPPORT is None: - raise NoTypeSupportImportedException() - - -def check_is_valid_msg_type(msg_type): - check_for_type_support(msg_type) - try: - assert None not in ( - msg_type.__class__._CREATE_ROS_MESSAGE, - msg_type.__class__._CONVERT_FROM_PY, - msg_type.__class__._CONVERT_TO_PY, - msg_type.__class__._DESTROY_ROS_MESSAGE, - ) - except (AssertionError, AttributeError): - raise RuntimeError( - f'The message type provided is not valid ({msg_type}),' - ' this might be a service or action' - ) from None - - -def check_is_valid_srv_type(srv_type): - check_for_type_support(srv_type) - try: - assert None not in ( - srv_type.Response, - srv_type.Request, - ) - except (AssertionError, AttributeError): - raise RuntimeError( - f'The service type provided is not valid ({srv_type}),' - ' this might be a message or action' - ) from None diff --git a/src/rclpy/utilities.py b/src/rclpy/utilities.py deleted file mode 100644 index ee7c868..0000000 --- a/src/rclpy/utilities.py +++ /dev/null @@ -1,135 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import sys -import threading - -import ament_index_python - -from rclpy.constants import S_TO_NS -from rclpy.context import Context - -g_default_context = None -g_context_lock = threading.Lock() - - -def get_default_context(*, shutting_down=False): - """Return the global default context singleton.""" - global g_context_lock - with g_context_lock: - global g_default_context - if g_default_context is None: - g_default_context = Context() - if shutting_down: - old_context = g_default_context - g_default_context = None - return old_context - return g_default_context - - -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( - args if args is not None else sys.argv) - - -def ok(*, context=None): - if context is None: - context = get_default_context() - return context.ok() - - -def shutdown(*, context=None): - if context is None: - context = get_default_context(shutting_down=True) - return context.shutdown() - - -def try_shutdown(*, context=None): - """Shutdown rclpy if not already shutdown.""" - global g_context_lock - global g_default_context - if context is None: - # Replace the default context with a new one if shutdown was successful - # or if the context was already shutdown. - with g_context_lock: - if g_default_context is None: - g_default_context = Context() - g_default_context.try_shutdown() - if not g_default_context.ok(): - g_default_context = None - else: - return context.try_shutdown() - - -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() - - -def get_available_rmw_implementations(): - """ - Return the set of all available RMW implementations as registered in the ament index. - - The result can be overridden by setting an environment variable named - ``RMW_IMPLEMENTATIONS``. - The variable can contain RMW implementation names separated by the platform - specific path separator. - Including an unavailable RMW implementation results in a RuntimeError. - """ - available_rmw_implementations = ament_index_python.get_resources( - 'rmw_typesupport') - available_rmw_implementations = { - name for name in available_rmw_implementations - if name != 'rmw_implementation'} - - # filter by implementations in environment variable if provided - rmw_implementations = os.environ.get('RMW_IMPLEMENTATIONS') - if rmw_implementations: - rmw_implementations = rmw_implementations.split(os.pathsep) - missing_rmw_implementations = set(rmw_implementations) - \ - available_rmw_implementations - if missing_rmw_implementations: - raise RuntimeError( - f'The RMW implementations {missing_rmw_implementations} ' - "specified in 'RMW_IMPLEMENTATIONS' are not available (" + - ', '.join(sorted(available_rmw_implementations)) + ')') - available_rmw_implementations = { - name for name in available_rmw_implementations - if name in rmw_implementations} - - return available_rmw_implementations - - -def timeout_sec_to_nsec(timeout_sec): - """ - Convert timeout in seconds to rcl compatible timeout in nanoseconds. - - Python tends to use floating point numbers in seconds for timeouts. This utility converts a - python-style timeout to an integer in nanoseconds that can be used by rcl_wait. - - :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if < 1ns - :type timeout_sec: float or None - :rtype: int - :returns: rcl_wait compatible timeout in nanoseconds - """ - if timeout_sec is None or timeout_sec < 0: - # Block forever - return -1 - else: - # wait for given time - return int(float(timeout_sec) * S_TO_NS) diff --git a/src/rclpy/validate_full_topic_name.py b/src/rclpy/validate_full_topic_name.py deleted file mode 100644 index afbf34b..0000000 --- a/src/rclpy/validate_full_topic_name.py +++ /dev/null @@ -1,41 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidServiceNameException -from rclpy.exceptions import InvalidTopicNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -def validate_full_topic_name(name, *, is_service=False): - """ - Validate a given topic or service name, and raise an exception if invalid. - - The name must be fully-qualified and already expanded. - - If the name is invalid then rclpy.exceptions.InvalidTopicNameException - will be raised. - - :param name str: topic or service name to be validated - :param is_service bool: if true, InvalidServiceNameException is raised - :returns: True when it is valid - :raises: InvalidTopicNameException: when the name is invalid - """ - result = _rclpy.rclpy_get_validation_error_for_full_topic_name(name) - if result is None: - return True - error_msg, invalid_index = result - if is_service: - raise InvalidServiceNameException(name, error_msg, invalid_index) - else: - raise InvalidTopicNameException(name, error_msg, invalid_index) diff --git a/src/rclpy/validate_namespace.py b/src/rclpy/validate_namespace.py deleted file mode 100644 index 3e1dcf6..0000000 --- a/src/rclpy/validate_namespace.py +++ /dev/null @@ -1,38 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidNamespaceException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -def validate_namespace(namespace): - """ - Validate a given namespace, and raise an exception if it is invalid. - - Unlike the node constructor, which allows namespaces without a leading '/' - and empty namespaces "", this function requires that the namespace be - absolute and non-empty. - - If the namespace is invalid then rclpy.exceptions.InvalidNamespaceException - will be raised. - - :param namespace str: namespace to be validated - :returns: True when it is valid - :raises: InvalidNamespaceException: when the namespace is invalid - """ - result = _rclpy.rclpy_get_validation_error_for_namespace(namespace) - if result is None: - return True - error_msg, invalid_index = result - raise InvalidNamespaceException(namespace, error_msg, invalid_index) diff --git a/src/rclpy/validate_node_name.py b/src/rclpy/validate_node_name.py deleted file mode 100644 index de14be4..0000000 --- a/src/rclpy/validate_node_name.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidNodeNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - - -def validate_node_name(node_name): - """ - Validate a given node_name, and raise an exception if it is invalid. - - If the node_name is invalid then rclpy.exceptions.InvalidNodeNameException - will be raised. - - :param node_name str: node_name to be validated - :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) - if result is None: - return True - error_msg, invalid_index = result - raise InvalidNodeNameException(node_name, error_msg, invalid_index) diff --git a/src/rclpy/validate_parameter_name.py b/src/rclpy/validate_parameter_name.py deleted file mode 100644 index e349d4a..0000000 --- a/src/rclpy/validate_parameter_name.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 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. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidParameterException - - -def validate_parameter_name(name: str) -> bool: - """ - Validate a given parameter name, and raise an exception if invalid. - - The name does not have to be fully-qualified and is not expanded. - - If the name is invalid then rclpy.exceptions.InvalidParameterException - will be raised. - - :param name str: parameter name to be validated. - :raises: InvalidParameterException: when the name is invalid. - """ - # TODO(jubeira): add parameter name check to be implemented at RCL level. - if not name: - raise InvalidParameterException(name) - - return True diff --git a/src/rclpy/validate_topic_name.py b/src/rclpy/validate_topic_name.py deleted file mode 100644 index 08712e4..0000000 --- a/src/rclpy/validate_topic_name.py +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from rclpy.exceptions import InvalidServiceNameException -from rclpy.exceptions import InvalidTopicNameException -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy - -TOPIC_SEPARATOR_STRING = '/' - - -def validate_topic_name(name, *, is_service=False): - """ - Validate a given topic or service name, and raise an exception if invalid. - - The name does not have to be fully-qualified and is not expanded. - - If the name is invalid then rclpy.exceptions.InvalidTopicNameException - will be raised. - - :param name str: topic or service name to be validated - :param is_service bool: if true, InvalidServiceNameException is raised - :returns: True when it is valid - :raises: InvalidTopicNameException: when the name is invalid - """ - result = _rclpy.rclpy_get_validation_error_for_topic_name(name) - if result is None: - return True - error_msg, invalid_index = result - if is_service: - raise InvalidServiceNameException(name, error_msg, invalid_index) - else: - raise InvalidTopicNameException(name, error_msg, invalid_index) diff --git a/src/rclpy/wait_for_message.py b/src/rclpy/wait_for_message.py deleted file mode 100644 index a26eb75..0000000 --- a/src/rclpy/wait_for_message.py +++ /dev/null @@ -1,71 +0,0 @@ -# Copyright 2022 Sony Group Corporation. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from typing import Union - -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy -from rclpy.node import Node -from rclpy.qos import QoSProfile -from rclpy.signals import SignalHandlerGuardCondition -from rclpy.utilities import timeout_sec_to_nsec - - -def wait_for_message( - msg_type, - node: 'Node', - topic: str, - *, - qos_profile: Union[QoSProfile, int] = 1, - time_to_wait=-1 -): - """ - Wait for the next incoming message. - - :param msg_type: message type - :param node: node to initialize the subscription on - :param topic: topic name to wait for message - :param qos_profile: QoS profile to use for the subscription - :param time_to_wait: seconds to wait before returning - :returns: (True, msg) if a message was successfully received, (False, None) if message - could not be obtained or shutdown was triggered asynchronously on the context. - """ - context = node.context - wait_set = _rclpy.WaitSet(1, 1, 0, 0, 0, 0, context.handle) - wait_set.clear_entities() - - sub = node.create_subscription(msg_type, topic, lambda _: None, qos_profile=qos_profile) - try: - wait_set.add_subscription(sub.handle) - sigint_gc = SignalHandlerGuardCondition(context=context) - wait_set.add_guard_condition(sigint_gc.handle) - - timeout_nsec = timeout_sec_to_nsec(time_to_wait) - wait_set.wait(timeout_nsec) - - subs_ready = wait_set.get_ready_entities('subscription') - guards_ready = wait_set.get_ready_entities('guard_condition') - - if guards_ready: - if sigint_gc.handle.pointer in guards_ready: - return False, None - - if subs_ready: - if sub.handle.pointer in subs_ready: - msg_info = sub.handle.take_message(sub.msg_type, sub.raw) - if msg_info is not None: - return True, msg_info[0] - finally: - node.destroy_subscription(sub) - - return False, None diff --git a/src/rclpy/waitable.py b/src/rclpy/waitable.py deleted file mode 100644 index fb70bb1..0000000 --- a/src/rclpy/waitable.py +++ /dev/null @@ -1,100 +0,0 @@ -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -class NumberOfEntities: - - __slots__ = [ - 'num_subscriptions', - 'num_guard_conditions', - 'num_timers', - 'num_clients', - 'num_services', - 'num_events'] - - def __init__( - self, num_subs=0, num_gcs=0, num_timers=0, - num_clients=0, num_services=0, num_events=0 - ): - self.num_subscriptions = num_subs - self.num_guard_conditions = num_gcs - self.num_timers = num_timers - self.num_clients = num_clients - self.num_services = num_services - self.num_events = num_events - - def __add__(self, other): - result = self.__class__() - for attr in result.__slots__: - left = getattr(self, attr) - right = getattr(other, attr) - setattr(result, attr, left + right) - return result - - def __repr__(self): - return '<{0}({1}, {2}, {3}, {4}, {5}, {6})>'.format( - self.__class__.__name__, self.num_subscriptions, - self.num_guard_conditions, self.num_timers, self.num_clients, - self.num_services, self.num_events) - - -class Waitable: - """ - Add something to a wait set and execute it. - - This class wraps a collection of entities which can be added to a wait set. - """ - - def __init__(self, callback_group): - # A callback group to control when this entity can execute (used by Executor) - self.callback_group = callback_group - self.callback_group.add_entity(self) - # Flag set by executor when a handler has been created but not executed (used by Executor) - self._executor_event = False - # List of Futures that have callbacks needing execution - self._futures = [] - - def __enter__(self): - """Implement to mark entities as in-use to prevent destruction while waiting on them.""" - pass - - def __exit__(self, t, v, tb): - """Implement to mark entities as not-in-use to allow destruction after waiting on them.""" - pass - - def add_future(self, future): - self._futures.append(future) - - def remove_future(self, future): - self._futures.remove(future) - - def is_ready(self, wait_set): - """Return True if entities are ready in the wait set.""" - raise NotImplementedError('Must be implemented by subclass') - - def take_data(self): - """Take stuff from lower level so the wait set doesn't immediately wake again.""" - raise NotImplementedError('Must be implemented by subclass') - - async def execute(self, taken_data): - """Execute work after data has been taken from a ready wait set.""" - raise NotImplementedError('Must be implemented by subclass') - - def get_num_entities(self): - """Return number of each type of entity used.""" - raise NotImplementedError('Must be implemented by subclass') - - def add_to_wait_set(self, wait_set): - """Add entities to wait set.""" - raise NotImplementedError('Must be implemented by subclass')