OpenSMI-ROS2 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- opensmi/ros2/__init__.py +89 -0
- opensmi/ros2/__main__.py +11 -0
- opensmi/ros2/cli.py +143 -0
- opensmi/ros2/py.typed +0 -0
- opensmi/ros2/runtime/__init__.py +67 -0
- opensmi/ros2/runtime/_decode.py +101 -0
- opensmi/ros2/runtime/_encode.py +102 -0
- opensmi/ros2/runtime/_parser.py +147 -0
- opensmi/ros2/runtime/errors.py +141 -0
- opensmi/ros2/runtime/goal.py +232 -0
- opensmi/ros2/runtime/graph.py +288 -0
- opensmi/ros2/runtime/guard.py +213 -0
- opensmi/ros2/runtime/interface.py +140 -0
- opensmi/ros2/runtime/messages/__init__.py +503 -0
- opensmi/ros2/runtime/messages/industrial_msgs.py +62 -0
- opensmi/ros2/runtime/messages/industrial_msgs.pyi +159 -0
- opensmi/ros2/runtime/messages/motoros2_interfaces.py +79 -0
- opensmi/ros2/runtime/messages/motoros2_interfaces.pyi +307 -0
- opensmi/ros2/runtime/messages/neo_msgs2.py +55 -0
- opensmi/ros2/runtime/messages/neo_msgs2.pyi +138 -0
- opensmi/ros2/runtime/messages/neo_srvs2.py +72 -0
- opensmi/ros2/runtime/messages/neo_srvs2.pyi +224 -0
- opensmi/ros2/runtime/outcomes.py +103 -0
- opensmi/ros2/runtime/parameters.py +723 -0
- opensmi/ros2/runtime/publisher.py +125 -0
- opensmi/ros2/runtime/qos.py +394 -0
- opensmi/ros2/runtime/runtime.py +1171 -0
- opensmi/ros2/runtime/selection.py +150 -0
- opensmi/ros2/runtime/stamp.py +100 -0
- opensmi/ros2/runtime/subscription.py +354 -0
- opensmi/ros2/runtime/tf.py +714 -0
- opensmi/ros2/server/__init__.py +148 -0
- opensmi/ros2/server/access.py +109 -0
- opensmi/ros2/server/bindings/__init__.py +92 -0
- opensmi/ros2/server/bindings/_base.py +154 -0
- opensmi/ros2/server/bindings/action.py +133 -0
- opensmi/ros2/server/bindings/keyed.py +127 -0
- opensmi/ros2/server/bindings/lifecycle.py +131 -0
- opensmi/ros2/server/bindings/method.py +130 -0
- opensmi/ros2/server/bindings/monitoring.py +109 -0
- opensmi/ros2/server/bindings/parameter.py +133 -0
- opensmi/ros2/server/bindings/publisher.py +192 -0
- opensmi/ros2/server/bindings/registry.py +95 -0
- opensmi/ros2/server/bindings/tf.py +256 -0
- opensmi/ros2/server/browse_names.py +94 -0
- opensmi/ros2/server/component.py +334 -0
- opensmi/ros2/server/config.py +493 -0
- opensmi/ros2/server/containers.py +191 -0
- opensmi/ros2/server/drivers.py +421 -0
- opensmi/ros2/server/errors.py +159 -0
- opensmi/ros2/server/events.py +406 -0
- opensmi/ros2/server/introspection.py +850 -0
- opensmi/ros2/server/keyed.py +749 -0
- opensmi/ros2/server/lifecycle.py +238 -0
- opensmi/ros2/server/limits.py +66 -0
- opensmi/ros2/server/loader.py +977 -0
- opensmi/ros2/server/machine.py +225 -0
- opensmi/ros2/server/method.py +307 -0
- opensmi/ros2/server/mirror.py +582 -0
- opensmi/ros2/server/mutation.py +992 -0
- opensmi/ros2/server/nodesets/Opc.Ua.SmartFactory.ROS2.NodeSet2.xml +85 -0
- opensmi/ros2/server/nodesets/__init__.py +18 -0
- opensmi/ros2/server/nodesets/smart_factory_ros2.py +26 -0
- opensmi/ros2/server/parameters.py +704 -0
- opensmi/ros2/server/projection.py +1176 -0
- opensmi/ros2/server/projections/__init__.py +41 -0
- opensmi/ros2/server/projections/scope.py +222 -0
- opensmi/ros2/server/provenance.py +121 -0
- opensmi/ros2/server/publisher.py +781 -0
- opensmi/ros2/server/recipes.py +514 -0
- opensmi/ros2/server/required.py +633 -0
- opensmi/ros2/server/skill.py +363 -0
- opensmi/ros2/server/spatial.py +283 -0
- opensmi/ros2/server/startup.py +626 -0
- opensmi/ros2/server/status.py +107 -0
- opensmi/ros2/server/structures.py +135 -0
- opensmi/ros2/server/transforms/__init__.py +39 -0
- opensmi/ros2/server/transforms/conversions.py +58 -0
- opensmi/ros2/server/transforms/filing.py +340 -0
- opensmi/ros2/server/transforms/mirror.py +462 -0
- opensmi/ros2/server/transforms/outward.py +180 -0
- opensmi/ros2/server/transforms/records.py +195 -0
- opensmi/ros2/server/transforms/spatial.py +154 -0
- opensmi_ros2-0.1.0.dist-info/METADATA +183 -0
- opensmi_ros2-0.1.0.dist-info/RECORD +89 -0
- opensmi_ros2-0.1.0.dist-info/WHEEL +5 -0
- opensmi_ros2-0.1.0.dist-info/entry_points.txt +2 -0
- opensmi_ros2-0.1.0.dist-info/licenses/LICENSES/MIT.txt +18 -0
- opensmi_ros2-0.1.0.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,1171 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The Runtime: the sole owner of ROS2 access for one Machine.
|
|
6
|
+
|
|
7
|
+
This is the only module in the package that imports ``rclpy``. One Runtime owns one ``rclpy``
|
|
8
|
+
Context on its own Domain ID, one Node and one executor thread. There is no registry and no
|
|
9
|
+
process global, so two Machines on two Domain IDs run in one process without discovering each
|
|
10
|
+
other.
|
|
11
|
+
|
|
12
|
+
No ``rclpy`` type crosses this interface. Every operation is async and must be awaited on the
|
|
13
|
+
loop :meth:`Runtime.start` ran on (otherwise it raises ``LoopAffinityError``). Callbacks that
|
|
14
|
+
``rclpy`` invokes on the executor thread only hop to the loop with ``call_soon_threadsafe``;
|
|
15
|
+
decoding and buffering happen there. Blocking ``rclpy`` calls run through ``asyncio.to_thread``.
|
|
16
|
+
QoS adaptation lives here too (see :mod:`opensmi.ros2.runtime.qos`).
|
|
17
|
+
"""
|
|
18
|
+
|
|
19
|
+
from __future__ import annotations
|
|
20
|
+
|
|
21
|
+
import asyncio
|
|
22
|
+
import threading
|
|
23
|
+
import warnings
|
|
24
|
+
from typing import TYPE_CHECKING, Any, Final, final
|
|
25
|
+
|
|
26
|
+
import rclpy.context
|
|
27
|
+
import rclpy.executors
|
|
28
|
+
import rclpy.node
|
|
29
|
+
import rclpy.qos
|
|
30
|
+
import structlog
|
|
31
|
+
import tf2_ros
|
|
32
|
+
from rclpy.action import ActionClient, get_action_names_and_types
|
|
33
|
+
from rclpy.duration import Duration
|
|
34
|
+
from rclpy.event_handler import SubscriptionEventCallbacks
|
|
35
|
+
from rclpy.time import Time
|
|
36
|
+
|
|
37
|
+
from opensmi.ros2.runtime._decode import decode
|
|
38
|
+
from opensmi.ros2.runtime._encode import encode
|
|
39
|
+
from opensmi.ros2.runtime._parser import import_action_class, import_interface_class, import_service_class
|
|
40
|
+
from opensmi.ros2.runtime.errors import (
|
|
41
|
+
ActionServerUnavailableError,
|
|
42
|
+
LoopAffinityError,
|
|
43
|
+
PublishFailedError,
|
|
44
|
+
RuntimeAlreadyStartedError,
|
|
45
|
+
RuntimeNotStartedError,
|
|
46
|
+
ServiceCallTimeoutError,
|
|
47
|
+
ServiceUnavailableError,
|
|
48
|
+
)
|
|
49
|
+
from opensmi.ros2.runtime.goal import Goal, GoalSink, rejected_outcome, settled_outcome
|
|
50
|
+
from opensmi.ros2.runtime.graph import POLL_INTERVAL, GraphFacet, GraphSnapshot
|
|
51
|
+
from opensmi.ros2.runtime.guard import require_sendable
|
|
52
|
+
from opensmi.ros2.runtime.messages import (
|
|
53
|
+
ActionTypes,
|
|
54
|
+
ServiceTypes,
|
|
55
|
+
action_types,
|
|
56
|
+
message_dataclass,
|
|
57
|
+
service_types,
|
|
58
|
+
)
|
|
59
|
+
from opensmi.ros2.runtime.outcomes import GoalOutcome, GoalStatus, StopOutcome
|
|
60
|
+
from opensmi.ros2.runtime.parameters import (
|
|
61
|
+
CALL_TIMEOUT,
|
|
62
|
+
DESCRIBE_PARAMETERS_TYPE,
|
|
63
|
+
EVENT_DEPTH,
|
|
64
|
+
GET_PARAMETERS_TYPE,
|
|
65
|
+
LIST_PARAMETERS_TYPE,
|
|
66
|
+
PARAMETER_EVENT_TYPE,
|
|
67
|
+
PARAMETER_EVENTS_TOPIC,
|
|
68
|
+
SERVER_TIMEOUT,
|
|
69
|
+
SET_PARAMETERS_TYPE,
|
|
70
|
+
ParameterDescription,
|
|
71
|
+
ParameterFacet,
|
|
72
|
+
ParameterReading,
|
|
73
|
+
ParameterValue,
|
|
74
|
+
SetOutcome,
|
|
75
|
+
describe_request,
|
|
76
|
+
description_of,
|
|
77
|
+
get_request,
|
|
78
|
+
list_request,
|
|
79
|
+
names_of,
|
|
80
|
+
outcomes_of,
|
|
81
|
+
reading_of,
|
|
82
|
+
set_request,
|
|
83
|
+
update_of,
|
|
84
|
+
)
|
|
85
|
+
from opensmi.ros2.runtime.publisher import Publisher
|
|
86
|
+
from opensmi.ros2.runtime.qos import (
|
|
87
|
+
DEFAULT_PROFILE,
|
|
88
|
+
FOREVER,
|
|
89
|
+
Durability,
|
|
90
|
+
EndpointInfo,
|
|
91
|
+
Liveliness,
|
|
92
|
+
Policy,
|
|
93
|
+
Provenance,
|
|
94
|
+
QoSNegotiation,
|
|
95
|
+
QoSProfile,
|
|
96
|
+
Reliability,
|
|
97
|
+
)
|
|
98
|
+
from opensmi.ros2.runtime.subscription import (
|
|
99
|
+
Subscription,
|
|
100
|
+
SubscriptionSink,
|
|
101
|
+
TypeHashMismatch,
|
|
102
|
+
TypeHashMismatchWarning,
|
|
103
|
+
)
|
|
104
|
+
from opensmi.ros2.runtime.tf import TRANSFORM_CACHE, TFFacet, Transform
|
|
105
|
+
|
|
106
|
+
if TYPE_CHECKING:
|
|
107
|
+
from collections.abc import AsyncGenerator, Awaitable, Callable, Iterable, Mapping
|
|
108
|
+
|
|
109
|
+
from opensmi.ros2.runtime.messages import Message
|
|
110
|
+
from opensmi.ros2.runtime.parameters import ParameterUpdate
|
|
111
|
+
|
|
112
|
+
__all__ = ["Runtime"]
|
|
113
|
+
|
|
114
|
+
# rclpy renders an unknown type hash as this literal; comparing against it would report every
|
|
115
|
+
# endpoint on a middleware that does not distribute hashes as drift.
|
|
116
|
+
_UNKNOWN_TYPE_HASH = "INVALID"
|
|
117
|
+
|
|
118
|
+
_LOGGER = structlog.get_logger("opensmi.ros2.runtime.runtime")
|
|
119
|
+
|
|
120
|
+
|
|
121
|
+
@final
|
|
122
|
+
class Runtime:
|
|
123
|
+
"""All ROS2 access for one Machine, which is the unit of one Domain ID."""
|
|
124
|
+
|
|
125
|
+
def __init__(
|
|
126
|
+
self,
|
|
127
|
+
*,
|
|
128
|
+
domain_id: int,
|
|
129
|
+
node_name: str = "opensmi_ros2",
|
|
130
|
+
namespace: str | None = None,
|
|
131
|
+
stop_timeout: float = 5.0,
|
|
132
|
+
graph_poll_interval: float = POLL_INTERVAL,
|
|
133
|
+
transform_cache: float = TRANSFORM_CACHE,
|
|
134
|
+
) -> None:
|
|
135
|
+
"""Describe a Runtime without starting anything; construction touches no ROS2.
|
|
136
|
+
|
|
137
|
+
:param domain_id: The ROS2 Domain ID this Runtime's Context is confined to.
|
|
138
|
+
:param node_name: The name of the single ``rclpy`` Node this Runtime owns.
|
|
139
|
+
:param namespace: The ROS2 namespace for that node, or ``None`` for the root.
|
|
140
|
+
:param stop_timeout: Seconds :meth:`stop` waits for the executor thread.
|
|
141
|
+
:param graph_poll_interval: Seconds between graph reads while the graph facet is watched.
|
|
142
|
+
:param transform_cache: Seconds of transform history the TF buffer keeps.
|
|
143
|
+
"""
|
|
144
|
+
self._domain_id = domain_id
|
|
145
|
+
self._node_name = node_name
|
|
146
|
+
self._namespace = namespace
|
|
147
|
+
self._stop_timeout = stop_timeout
|
|
148
|
+
self._context: rclpy.context.Context | None = None
|
|
149
|
+
self._node: rclpy.node.Node | None = None
|
|
150
|
+
self._executor: rclpy.executors.SingleThreadedExecutor | None = None
|
|
151
|
+
self._thread: threading.Thread | None = None
|
|
152
|
+
self._loop: asyncio.AbstractEventLoop | None = None
|
|
153
|
+
self._subscriptions: list[Subscription] = []
|
|
154
|
+
self._publishers: list[Publisher] = []
|
|
155
|
+
self._goals: list[Goal] = []
|
|
156
|
+
self._transform_cache = transform_cache
|
|
157
|
+
self._buffer: tf2_ros.Buffer | None = None
|
|
158
|
+
self._graph = GraphFacet(read=self._read_graph, interval=graph_poll_interval)
|
|
159
|
+
self._tf = TFFacet(
|
|
160
|
+
subscribe=self.subscribe,
|
|
161
|
+
advertise=self.advertise,
|
|
162
|
+
feed=self._feed_transforms,
|
|
163
|
+
lookup=self._lookup_transform,
|
|
164
|
+
clock=self._ros2_clock,
|
|
165
|
+
)
|
|
166
|
+
self._parameters = ParameterFacet(
|
|
167
|
+
read=self._read_parameter,
|
|
168
|
+
write=self._write_parameters,
|
|
169
|
+
events=self._follow_parameter_events,
|
|
170
|
+
list_names=self._list_parameters,
|
|
171
|
+
describe=self._describe_parameter,
|
|
172
|
+
)
|
|
173
|
+
|
|
174
|
+
def __repr__(self) -> str:
|
|
175
|
+
"""Return a representation naming the node and its Domain ID."""
|
|
176
|
+
state = "started" if self.started else "stopped"
|
|
177
|
+
return f"<Runtime {self._node_name} on domain {self._domain_id} ({state})>"
|
|
178
|
+
|
|
179
|
+
@property
|
|
180
|
+
def domain_id(self) -> int:
|
|
181
|
+
"""Return the ROS2 Domain ID this Runtime is confined to."""
|
|
182
|
+
return self._domain_id
|
|
183
|
+
|
|
184
|
+
@property
|
|
185
|
+
def node_name(self) -> str:
|
|
186
|
+
"""Return the name of the single ``rclpy`` Node this Runtime owns."""
|
|
187
|
+
return self._node_name
|
|
188
|
+
|
|
189
|
+
@property
|
|
190
|
+
def started(self) -> bool:
|
|
191
|
+
"""Return whether the Context, Node and executor thread are up."""
|
|
192
|
+
return self._context is not None
|
|
193
|
+
|
|
194
|
+
@property
|
|
195
|
+
def graph(self) -> GraphFacet:
|
|
196
|
+
"""Return the facet answering what is on the ROS2 graph (it reads nothing until watched)."""
|
|
197
|
+
return self._graph
|
|
198
|
+
|
|
199
|
+
@property
|
|
200
|
+
def parameters(self) -> ParameterFacet:
|
|
201
|
+
"""Return the facet for the parameters of ROS2 nodes (``/parameter_events`` only while watched)."""
|
|
202
|
+
return self._parameters
|
|
203
|
+
|
|
204
|
+
@property
|
|
205
|
+
def tf(self) -> TFFacet:
|
|
206
|
+
"""Return the facet answering where a TF frame sits (``/tf`` is subscribed once a Binding asks)."""
|
|
207
|
+
return self._tf
|
|
208
|
+
|
|
209
|
+
async def start(self) -> None:
|
|
210
|
+
"""Bring up the Context, the Node and the executor thread on the running loop, which stays the Runtime's.
|
|
211
|
+
|
|
212
|
+
:raises RuntimeAlreadyStartedError: When the Runtime is already up.
|
|
213
|
+
"""
|
|
214
|
+
if self.started:
|
|
215
|
+
message = f"{self._node_name} is already started on domain {self._domain_id}"
|
|
216
|
+
raise RuntimeAlreadyStartedError(message)
|
|
217
|
+
loop = asyncio.get_running_loop()
|
|
218
|
+
await asyncio.to_thread(self._open)
|
|
219
|
+
self._loop = loop
|
|
220
|
+
|
|
221
|
+
def _open(self) -> None:
|
|
222
|
+
"""Create and spin the ROS2 objects (blocking, so called off the loop)."""
|
|
223
|
+
context = rclpy.context.Context()
|
|
224
|
+
try:
|
|
225
|
+
context.init(args=None, domain_id=self._domain_id)
|
|
226
|
+
node = rclpy.node.Node(self._node_name, context=context, namespace=self._namespace)
|
|
227
|
+
executor = rclpy.executors.SingleThreadedExecutor(context=context)
|
|
228
|
+
executor.add_node(node)
|
|
229
|
+
except BaseException:
|
|
230
|
+
context.try_shutdown()
|
|
231
|
+
raise
|
|
232
|
+
# A daemon thread, so a process that never stops its Runtime still exits.
|
|
233
|
+
thread = threading.Thread(
|
|
234
|
+
target=executor.spin,
|
|
235
|
+
name=f"ros2-runtime-domain-{self._domain_id}",
|
|
236
|
+
daemon=True,
|
|
237
|
+
)
|
|
238
|
+
thread.start()
|
|
239
|
+
self._context = context
|
|
240
|
+
self._node = node
|
|
241
|
+
self._executor = executor
|
|
242
|
+
self._thread = thread
|
|
243
|
+
# No `TransformListener`: it would spin this Node on a second executor thread. A bare
|
|
244
|
+
# `Buffer` is fed by the TF facet's own subscriptions.
|
|
245
|
+
self._buffer = tf2_ros.Buffer(cache_time=Duration(seconds=self._transform_cache))
|
|
246
|
+
|
|
247
|
+
async def stop(self) -> StopOutcome:
|
|
248
|
+
"""Close every open endpoint, then tear down the thread, the Node and the Context.
|
|
249
|
+
|
|
250
|
+
Teardown faults are returned in the outcome, not raised: the Runtime is stopped either way.
|
|
251
|
+
|
|
252
|
+
:raises RuntimeNotStartedError: When the Runtime was never started, or is already stopped.
|
|
253
|
+
:raises LoopAffinityError: When awaited on a loop other than the Runtime's.
|
|
254
|
+
"""
|
|
255
|
+
self._require_started()
|
|
256
|
+
self._require_loop()
|
|
257
|
+
await self._graph.close()
|
|
258
|
+
# Before the subscriptions below: these facets own some of them.
|
|
259
|
+
await self._tf.close()
|
|
260
|
+
await self._parameters.close()
|
|
261
|
+
for goal in tuple(self._goals):
|
|
262
|
+
await goal.close()
|
|
263
|
+
self._goals.clear()
|
|
264
|
+
for subscription in tuple(self._subscriptions):
|
|
265
|
+
await subscription.close()
|
|
266
|
+
self._subscriptions.clear()
|
|
267
|
+
for publisher in tuple(self._publishers):
|
|
268
|
+
await publisher.close()
|
|
269
|
+
self._publishers.clear()
|
|
270
|
+
return await asyncio.to_thread(self._close)
|
|
271
|
+
|
|
272
|
+
def _close(self) -> StopOutcome:
|
|
273
|
+
"""Tear the ROS2 objects down (blocking, so called off the loop), collecting faults."""
|
|
274
|
+
executor, node, context, thread = self._executor, self._node, self._context, self._thread
|
|
275
|
+
self._executor = self._node = self._context = self._thread = self._loop = None
|
|
276
|
+
self._buffer = None
|
|
277
|
+
faults: list[str] = []
|
|
278
|
+
joined = False
|
|
279
|
+
if executor is not None:
|
|
280
|
+
faults += _faults_from(
|
|
281
|
+
"shutting the executor down",
|
|
282
|
+
lambda: executor.shutdown(timeout_sec=self._stop_timeout),
|
|
283
|
+
)
|
|
284
|
+
if thread is not None:
|
|
285
|
+
thread.join(timeout=self._stop_timeout)
|
|
286
|
+
joined = not thread.is_alive()
|
|
287
|
+
if not joined:
|
|
288
|
+
faults.append(f"the executor thread did not end within {self._stop_timeout} seconds")
|
|
289
|
+
if node is not None:
|
|
290
|
+
faults += _faults_from("destroying the node", node.destroy_node)
|
|
291
|
+
if context is not None:
|
|
292
|
+
faults += _faults_from("shutting the context down", context.try_shutdown)
|
|
293
|
+
return StopOutcome(faults=tuple(faults), executor_thread_joined=joined)
|
|
294
|
+
|
|
295
|
+
async def subscribe(
|
|
296
|
+
self,
|
|
297
|
+
*,
|
|
298
|
+
topic: str,
|
|
299
|
+
message_type: str | type[Message],
|
|
300
|
+
depth: int = 10,
|
|
301
|
+
qos: QoSProfile | None = None,
|
|
302
|
+
) -> Subscription:
|
|
303
|
+
"""Subscribe to a ROS2 topic, and return the handle that owns the subscription.
|
|
304
|
+
|
|
305
|
+
A declared profile is honoured exactly; a discovered one relaxes as the topic's publishers
|
|
306
|
+
require. The call does not wait for a publisher: it opens against whatever the graph shows
|
|
307
|
+
now, possibly nothing.
|
|
308
|
+
|
|
309
|
+
:param topic: The ROS2 topic name.
|
|
310
|
+
:param message_type: The ROS2 type name (``package/Name`` or ``package/kind/Name``) or an
|
|
311
|
+
already-resolved message dataclass.
|
|
312
|
+
:param depth: The handle's buffer size, and the ROS2 queue depth.
|
|
313
|
+
:param qos: The profile to request, or ``None`` to discover one: the most permissive
|
|
314
|
+
request compatible with every publisher now, re-resolved once on the first publisher
|
|
315
|
+
when there is none yet, and relaxed whenever the middleware reports an incompatibility.
|
|
316
|
+
:raises RuntimeNotStartedError: When the Runtime is not up.
|
|
317
|
+
:raises LoopAffinityError: When awaited on a loop other than the Runtime's.
|
|
318
|
+
:raises MissingInterfacePackageError: When the type's interface package is not installed.
|
|
319
|
+
"""
|
|
320
|
+
node = self._require_started()
|
|
321
|
+
loop = self._require_loop()
|
|
322
|
+
dataclass_type = self.describe_message(message_type) if isinstance(message_type, str) else message_type
|
|
323
|
+
interface_class = import_interface_class(dataclass_type.ros2.type_name)
|
|
324
|
+
|
|
325
|
+
negotiation = QoSNegotiation(declared=qos, endpoints=self.publishers_on(topic))
|
|
326
|
+
sink = SubscriptionSink(depth=depth, qos=negotiation.profile, provenance=negotiation.provenance)
|
|
327
|
+
|
|
328
|
+
async def release() -> None:
|
|
329
|
+
await adaptive.close()
|
|
330
|
+
if handle in self._subscriptions:
|
|
331
|
+
self._subscriptions.remove(handle)
|
|
332
|
+
|
|
333
|
+
handle = Subscription(topic=topic, message_type=dataclass_type, sink=sink, release=release)
|
|
334
|
+
|
|
335
|
+
def on_loop(raw: Any) -> None:
|
|
336
|
+
sink.deliver(decode(raw, dataclass_type))
|
|
337
|
+
|
|
338
|
+
def on_message(raw: Any) -> None:
|
|
339
|
+
# Executor thread: anything heavier than the hop stalls the wait set.
|
|
340
|
+
loop.call_soon_threadsafe(on_loop, raw)
|
|
341
|
+
|
|
342
|
+
def on_matched(info: Any) -> None:
|
|
343
|
+
loop.call_soon_threadsafe(adaptive.matched, int(info.current_count))
|
|
344
|
+
|
|
345
|
+
def on_incompatible(info: Any) -> None:
|
|
346
|
+
# `last_policy_kind` is only the last clash the middleware found.
|
|
347
|
+
loop.call_soon_threadsafe(adaptive.incompatible, _policy_of(info.last_policy_kind))
|
|
348
|
+
|
|
349
|
+
async def create(profile: QoSProfile) -> Any:
|
|
350
|
+
return await asyncio.to_thread(
|
|
351
|
+
node.create_subscription,
|
|
352
|
+
interface_class,
|
|
353
|
+
topic,
|
|
354
|
+
on_message,
|
|
355
|
+
_rclpy_profile(profile, depth),
|
|
356
|
+
event_callbacks=SubscriptionEventCallbacks(matched=on_matched, incompatible_qos=on_incompatible),
|
|
357
|
+
)
|
|
358
|
+
|
|
359
|
+
async def destroy(ros_subscription: Any) -> None:
|
|
360
|
+
await asyncio.to_thread(self._destroy, "subscription", ros_subscription)
|
|
361
|
+
|
|
362
|
+
adaptive = _AdaptiveSubscription(
|
|
363
|
+
topic=topic,
|
|
364
|
+
negotiation=negotiation,
|
|
365
|
+
sink=sink,
|
|
366
|
+
loop=loop,
|
|
367
|
+
create=create,
|
|
368
|
+
destroy=destroy,
|
|
369
|
+
publishers=lambda: self.publishers_on(topic),
|
|
370
|
+
)
|
|
371
|
+
await adaptive.open()
|
|
372
|
+
self._subscriptions.append(handle)
|
|
373
|
+
self._report_type_hash_mismatches(topic, sink)
|
|
374
|
+
return handle
|
|
375
|
+
|
|
376
|
+
def publishers_on(self, topic: str) -> tuple[EndpointInfo, ...]:
|
|
377
|
+
"""Return what every publisher on a topic offers, as the QoS resolver takes it.
|
|
378
|
+
|
|
379
|
+
This reads the node's local graph cache on the loop: callers decide synchronously whether
|
|
380
|
+
to adapt, and an ``await`` in between would let two match events spend the one bootstrap
|
|
381
|
+
tightening twice. Returns an empty tuple when the Runtime is not up, because middleware
|
|
382
|
+
callbacks call it during teardown.
|
|
383
|
+
"""
|
|
384
|
+
node = self._node
|
|
385
|
+
if node is None:
|
|
386
|
+
return ()
|
|
387
|
+
return tuple(
|
|
388
|
+
EndpointInfo(
|
|
389
|
+
node_name=f"{info.node_namespace.rstrip('/')}/{info.node_name}",
|
|
390
|
+
qos=_our_profile(info.qos_profile),
|
|
391
|
+
)
|
|
392
|
+
for info in node.get_publishers_info_by_topic(topic)
|
|
393
|
+
)
|
|
394
|
+
|
|
395
|
+
async def advertise(
|
|
396
|
+
self,
|
|
397
|
+
*,
|
|
398
|
+
topic: str,
|
|
399
|
+
message_type: str | type[Message],
|
|
400
|
+
depth: int = 10,
|
|
401
|
+
qos: QoSProfile | None = None,
|
|
402
|
+
) -> Publisher:
|
|
403
|
+
"""Advertise a ROS2 topic, and return the handle that owns the publisher.
|
|
404
|
+
|
|
405
|
+
Nothing goes on the wire until :meth:`~opensmi.ros2.runtime.publisher.Publisher.publish`.
|
|
406
|
+
The profile is declared or defaulted, never discovered or adapted, since this call is how
|
|
407
|
+
a publisher on the topic comes to exist.
|
|
408
|
+
|
|
409
|
+
:param topic: The ROS2 topic name.
|
|
410
|
+
:param message_type: The ROS2 type name (``package/Name`` or ``package/kind/Name``) or an
|
|
411
|
+
already-resolved message dataclass.
|
|
412
|
+
:param depth: The ROS2 queue depth.
|
|
413
|
+
:param qos: The profile to offer, or ``None`` for
|
|
414
|
+
:data:`~opensmi.ros2.runtime.qos.DEFAULT_PROFILE`.
|
|
415
|
+
:raises RuntimeNotStartedError: When the Runtime is not up.
|
|
416
|
+
:raises LoopAffinityError: When awaited on a loop other than the Runtime's.
|
|
417
|
+
:raises MissingInterfacePackageError: When the type's interface package is not installed.
|
|
418
|
+
"""
|
|
419
|
+
node = self._require_started()
|
|
420
|
+
self._require_loop()
|
|
421
|
+
dataclass_type = self.describe_message(message_type) if isinstance(message_type, str) else message_type
|
|
422
|
+
interface_class = import_interface_class(dataclass_type.ros2.type_name)
|
|
423
|
+
profile = qos if qos is not None else DEFAULT_PROFILE
|
|
424
|
+
provenance = Provenance.DECLARED if qos is not None else Provenance.DEFAULTED
|
|
425
|
+
ros_publisher: Any = None
|
|
426
|
+
|
|
427
|
+
def put(message: Message) -> None:
|
|
428
|
+
ros_publisher.publish(encode(message))
|
|
429
|
+
|
|
430
|
+
async def send(message: Message) -> None:
|
|
431
|
+
# Off the loop: encoding a large occupancy grid would stall the OPC UA endpoint.
|
|
432
|
+
# Faults are wrapped because rclpy spells "the publish did not happen" several ways.
|
|
433
|
+
try:
|
|
434
|
+
await asyncio.to_thread(put, message)
|
|
435
|
+
except Exception as error:
|
|
436
|
+
text = f"rclpy refused to publish on {topic}: {error}"
|
|
437
|
+
raise PublishFailedError(text) from error
|
|
438
|
+
|
|
439
|
+
async def release() -> None:
|
|
440
|
+
if ros_publisher is not None:
|
|
441
|
+
await asyncio.to_thread(self._destroy, "publisher", ros_publisher)
|
|
442
|
+
if handle in self._publishers:
|
|
443
|
+
self._publishers.remove(handle)
|
|
444
|
+
|
|
445
|
+
handle = Publisher(
|
|
446
|
+
topic=topic,
|
|
447
|
+
message_type=dataclass_type,
|
|
448
|
+
clock=self._ros2_clock,
|
|
449
|
+
send=send,
|
|
450
|
+
release=release,
|
|
451
|
+
qos=profile,
|
|
452
|
+
provenance=provenance,
|
|
453
|
+
)
|
|
454
|
+
ros_publisher = await asyncio.to_thread(
|
|
455
|
+
node.create_publisher,
|
|
456
|
+
interface_class,
|
|
457
|
+
topic,
|
|
458
|
+
_rclpy_profile(profile, depth),
|
|
459
|
+
)
|
|
460
|
+
self._publishers.append(handle)
|
|
461
|
+
return handle
|
|
462
|
+
|
|
463
|
+
def _feed_transforms(self, message: Message, static: bool) -> None:
|
|
464
|
+
"""Put every transform of one message into the buffer, on the loop that received it.
|
|
465
|
+
|
|
466
|
+
The authority is this Runtime's node name, as a ``TransformListener`` would have written
|
|
467
|
+
it. Nothing is raised: tf2 refuses a non-finite value or an empty frame id, and that is no
|
|
468
|
+
reason to stop reading the topic.
|
|
469
|
+
"""
|
|
470
|
+
buffer = self._buffer
|
|
471
|
+
if buffer is None: # pragma: no cover -- the facet is closed before the buffer is dropped
|
|
472
|
+
return
|
|
473
|
+
put = buffer.set_transform_static if static else buffer.set_transform
|
|
474
|
+
for transform in getattr(encode(message), "transforms", ()):
|
|
475
|
+
try:
|
|
476
|
+
put(transform, self._node_name)
|
|
477
|
+
except Exception as error:
|
|
478
|
+
_LOGGER.debug("tf2 refused a transform", error=str(error))
|
|
479
|
+
|
|
480
|
+
def _lookup_transform(self, target: str, source: str) -> Transform | None:
|
|
481
|
+
"""Return where one frame sits in another, out of what the buffer already holds.
|
|
482
|
+
|
|
483
|
+
Non-blocking: ``lookup_transform`` at zero time returns the latest cached transform or
|
|
484
|
+
raises. The ``_async`` variant is avoided because awaiting its future on this loop
|
|
485
|
+
busy-spins until another message feeds the buffer.
|
|
486
|
+
|
|
487
|
+
:return: The composed transform, or ``None`` when the buffer cannot complete the chain
|
|
488
|
+
(unpublished frame, two trees that do not meet, or a link older than the cache).
|
|
489
|
+
"""
|
|
490
|
+
buffer = self._buffer
|
|
491
|
+
if buffer is None: # pragma: no cover -- the facet is closed before the buffer is dropped
|
|
492
|
+
return None
|
|
493
|
+
try:
|
|
494
|
+
stamped = buffer.lookup_transform(target, source, Time())
|
|
495
|
+
except tf2_ros.TransformException:
|
|
496
|
+
return None
|
|
497
|
+
return Transform(
|
|
498
|
+
translation=(
|
|
499
|
+
float(stamped.transform.translation.x),
|
|
500
|
+
float(stamped.transform.translation.y),
|
|
501
|
+
float(stamped.transform.translation.z),
|
|
502
|
+
),
|
|
503
|
+
rotation=(
|
|
504
|
+
float(stamped.transform.rotation.x),
|
|
505
|
+
float(stamped.transform.rotation.y),
|
|
506
|
+
float(stamped.transform.rotation.z),
|
|
507
|
+
float(stamped.transform.rotation.w),
|
|
508
|
+
),
|
|
509
|
+
)
|
|
510
|
+
|
|
511
|
+
def _ros2_clock(self) -> tuple[int, int]:
|
|
512
|
+
"""Return the Node's clock (not the wall clock, so right under ``use_sim_time``) as seconds and nanoseconds."""
|
|
513
|
+
seconds, nanoseconds = self._require_started().get_clock().now().seconds_nanoseconds()
|
|
514
|
+
return int(seconds), int(nanoseconds)
|
|
515
|
+
|
|
516
|
+
def describe_message(self, message_type: str) -> type[Message]:
|
|
517
|
+
"""Return the message dataclass of a ROS2 message type.
|
|
518
|
+
|
|
519
|
+
Usable before :meth:`start`.
|
|
520
|
+
|
|
521
|
+
:param message_type: ``package/Name`` or ``package/msg/Name``.
|
|
522
|
+
:raises InvalidMessageTypeNameError: When the name is not a message name.
|
|
523
|
+
:raises MissingInterfacePackageError: When the interface package is not installed.
|
|
524
|
+
:raises UnusableMessageTypeError: When the package has no such message.
|
|
525
|
+
"""
|
|
526
|
+
return message_dataclass(message_type)
|
|
527
|
+
|
|
528
|
+
def default_message(self, message_type: type[Message]) -> Message:
|
|
529
|
+
"""Return a message of a type as its generated ROS2 class constructs it, with no arguments.
|
|
530
|
+
|
|
531
|
+
Every field holds the default its interface definition declares, or its type's zero. Usable
|
|
532
|
+
before :meth:`start`.
|
|
533
|
+
|
|
534
|
+
:param message_type: A message dataclass, as :meth:`describe_message` returns it.
|
|
535
|
+
"""
|
|
536
|
+
return decode(import_interface_class(message_type.ros2.type_name)(), message_type)
|
|
537
|
+
|
|
538
|
+
def describe_action(self, action_type: str) -> ActionTypes:
|
|
539
|
+
"""Return the goal, feedback and result message dataclasses of a ROS2 action type.
|
|
540
|
+
|
|
541
|
+
Usable before :meth:`start`, so a missing interface package fails while a Component is
|
|
542
|
+
constructed. It lives on the Runtime so that tests can substitute a double.
|
|
543
|
+
|
|
544
|
+
:param action_type: ``package/action/Name``.
|
|
545
|
+
:raises InvalidMessageTypeNameError: When the name is not a three-part action name.
|
|
546
|
+
:raises MissingInterfacePackageError: When the interface package is not installed.
|
|
547
|
+
:raises UnusableMessageTypeError: When the package has no such action.
|
|
548
|
+
"""
|
|
549
|
+
return action_types(action_type)
|
|
550
|
+
|
|
551
|
+
def describe_service(self, service_type: str) -> ServiceTypes:
|
|
552
|
+
"""Return the request and response message dataclasses of a ROS2 service type.
|
|
553
|
+
|
|
554
|
+
Usable before :meth:`start`, like :meth:`describe_action`.
|
|
555
|
+
|
|
556
|
+
:param service_type: ``package/srv/Name``.
|
|
557
|
+
:raises InvalidMessageTypeNameError: When the name is not a three-part service name.
|
|
558
|
+
:raises MissingInterfacePackageError: When the interface package is not installed.
|
|
559
|
+
:raises UnusableMessageTypeError: When the package has no such service.
|
|
560
|
+
"""
|
|
561
|
+
return service_types(service_type)
|
|
562
|
+
|
|
563
|
+
async def call_service(
|
|
564
|
+
self,
|
|
565
|
+
*,
|
|
566
|
+
service: str,
|
|
567
|
+
service_type: str | ServiceTypes,
|
|
568
|
+
request: Message,
|
|
569
|
+
server_timeout: float = 10.0,
|
|
570
|
+
call_timeout: float = 30.0,
|
|
571
|
+
) -> Message:
|
|
572
|
+
"""Call one ROS2 service and return the response it answered with.
|
|
573
|
+
|
|
574
|
+
A refusal is in the response message. Only two cases raise: nobody serving the path, and
|
|
575
|
+
a server that took the request and never answered. They have separate timeouts so that
|
|
576
|
+
"the robot is off" and "the robot is stuck" stay distinguishable.
|
|
577
|
+
|
|
578
|
+
:param service: The ROS2 service name.
|
|
579
|
+
:param service_type: The service type (``package/srv/Name``) or the already-resolved
|
|
580
|
+
pair of message dataclasses.
|
|
581
|
+
:param request: The request, as an instance of the service's request message dataclass.
|
|
582
|
+
:param server_timeout: Seconds to wait for a service server to appear on the path.
|
|
583
|
+
:param call_timeout: Seconds to wait for the answer once the request has gone.
|
|
584
|
+
:raises RuntimeNotStartedError: When the Runtime is not up.
|
|
585
|
+
:raises LoopAffinityError: When awaited on a loop other than the Runtime's.
|
|
586
|
+
:raises FieldTypeMismatchError: When a field of the request holds a value of the wrong
|
|
587
|
+
Python type. Refused here because ``rclpy`` aborts the process.
|
|
588
|
+
:raises FieldValueOutOfRangeError: When a field of the request is past its width or bound.
|
|
589
|
+
:raises ServiceUnavailableError: When no service server answers on the path.
|
|
590
|
+
:raises ServiceCallTimeoutError: When the request went and no answer came back in time.
|
|
591
|
+
:raises MissingInterfacePackageError: When the service's interface package is absent.
|
|
592
|
+
"""
|
|
593
|
+
node = self._require_started()
|
|
594
|
+
loop = self._require_loop()
|
|
595
|
+
types = self.describe_service(service_type) if isinstance(service_type, str) else service_type
|
|
596
|
+
_require_the_payload_type("request", service, request, types.request, types.type_name)
|
|
597
|
+
require_sendable(request)
|
|
598
|
+
|
|
599
|
+
service_class = import_service_class(types.type_name)
|
|
600
|
+
client = await asyncio.to_thread(node.create_client, service_class, service)
|
|
601
|
+
try:
|
|
602
|
+
if not await asyncio.to_thread(client.wait_for_service, server_timeout):
|
|
603
|
+
_refuse_an_unserved_service(service, server_timeout)
|
|
604
|
+
pending = await asyncio.to_thread(client.call_async, encode(request))
|
|
605
|
+
try:
|
|
606
|
+
raw = await asyncio.wait_for(_resolved(pending, loop), timeout=call_timeout)
|
|
607
|
+
except TimeoutError as error:
|
|
608
|
+
message = f"the service on {service} took the request and did not answer within {call_timeout} seconds"
|
|
609
|
+
raise ServiceCallTimeoutError(message) from error
|
|
610
|
+
finally:
|
|
611
|
+
await asyncio.to_thread(self._destroy, "client", client)
|
|
612
|
+
return decode(raw, types.response)
|
|
613
|
+
|
|
614
|
+
async def send_goal(
|
|
615
|
+
self,
|
|
616
|
+
*,
|
|
617
|
+
action: str,
|
|
618
|
+
action_type: str | ActionTypes,
|
|
619
|
+
goal: Message,
|
|
620
|
+
depth: int = 10,
|
|
621
|
+
server_timeout: float = 10.0,
|
|
622
|
+
) -> Goal:
|
|
623
|
+
"""Send one goal to a ROS2 action server, and return the handle that owns it.
|
|
624
|
+
|
|
625
|
+
The handle is returned once the server has answered the goal request. A refusal comes back
|
|
626
|
+
as an already settled handle; only a path nobody serves raises.
|
|
627
|
+
|
|
628
|
+
:param action: The ROS2 action name.
|
|
629
|
+
:param action_type: The action type (``package/action/Name``) or the already-resolved
|
|
630
|
+
trio of message dataclasses.
|
|
631
|
+
:param goal: The goal, as an instance of the action's goal message dataclass.
|
|
632
|
+
:param depth: How many feedback messages the handle buffers.
|
|
633
|
+
:param server_timeout: Seconds to wait for an action server to appear on the path.
|
|
634
|
+
:raises RuntimeNotStartedError: When the Runtime is not up.
|
|
635
|
+
:raises LoopAffinityError: When awaited on a loop other than the Runtime's.
|
|
636
|
+
:raises FieldTypeMismatchError: When a field of the goal holds a value of the wrong
|
|
637
|
+
Python type. Refused here because ``rclpy`` aborts the process instead of raising.
|
|
638
|
+
:raises FieldValueOutOfRangeError: When a field of the goal is past its width or bound.
|
|
639
|
+
:raises ActionServerUnavailableError: When no action server answers on the path.
|
|
640
|
+
:raises MissingInterfacePackageError: When the action's interface package is absent.
|
|
641
|
+
"""
|
|
642
|
+
node = self._require_started()
|
|
643
|
+
loop = self._require_loop()
|
|
644
|
+
types = self.describe_action(action_type) if isinstance(action_type, str) else action_type
|
|
645
|
+
_require_the_payload_type("goal", action, goal, types.goal, types.type_name)
|
|
646
|
+
require_sendable(goal)
|
|
647
|
+
|
|
648
|
+
action_class = import_action_class(types.type_name)
|
|
649
|
+
sink = GoalSink(depth=depth)
|
|
650
|
+
client = await asyncio.to_thread(ActionClient, node, action_class, action)
|
|
651
|
+
try:
|
|
652
|
+
if not await asyncio.to_thread(client.wait_for_server, server_timeout):
|
|
653
|
+
_refuse_an_unserved_action(action, server_timeout)
|
|
654
|
+
handle = await self._await_acceptance(
|
|
655
|
+
action=action,
|
|
656
|
+
types=types,
|
|
657
|
+
goal=goal,
|
|
658
|
+
sink=sink,
|
|
659
|
+
client=client,
|
|
660
|
+
loop=loop,
|
|
661
|
+
)
|
|
662
|
+
except BaseException:
|
|
663
|
+
await asyncio.to_thread(self._destroy_action_client, client)
|
|
664
|
+
raise
|
|
665
|
+
self._goals.append(handle)
|
|
666
|
+
return handle
|
|
667
|
+
|
|
668
|
+
async def _await_acceptance(
|
|
669
|
+
self,
|
|
670
|
+
*,
|
|
671
|
+
action: str,
|
|
672
|
+
types: ActionTypes,
|
|
673
|
+
goal: Message,
|
|
674
|
+
sink: GoalSink,
|
|
675
|
+
client: Any,
|
|
676
|
+
loop: asyncio.AbstractEventLoop,
|
|
677
|
+
) -> Goal:
|
|
678
|
+
"""Send the goal, wait for the server's answer, and build the handle around it."""
|
|
679
|
+
|
|
680
|
+
def on_feedback(raw: Any) -> None:
|
|
681
|
+
# `raw` is the generated FeedbackMessage wrapper; the feedback is inside it.
|
|
682
|
+
loop.call_soon_threadsafe(_deliver, raw.feedback)
|
|
683
|
+
|
|
684
|
+
def _deliver(raw: Any) -> None:
|
|
685
|
+
sink.deliver(decode(raw, types.feedback))
|
|
686
|
+
|
|
687
|
+
ros_goal = encode(goal)
|
|
688
|
+
goal_handle = await _resolved(
|
|
689
|
+
await asyncio.to_thread(client.send_goal_async, ros_goal, on_feedback),
|
|
690
|
+
loop,
|
|
691
|
+
)
|
|
692
|
+
|
|
693
|
+
async def request_cancel() -> None:
|
|
694
|
+
await _resolved(await asyncio.to_thread(goal_handle.cancel_goal_async), loop)
|
|
695
|
+
|
|
696
|
+
async def release() -> None:
|
|
697
|
+
await asyncio.to_thread(self._destroy_action_client, client)
|
|
698
|
+
if handle in self._goals:
|
|
699
|
+
self._goals.remove(handle)
|
|
700
|
+
|
|
701
|
+
handle = Goal(
|
|
702
|
+
action=action,
|
|
703
|
+
types=types,
|
|
704
|
+
sink=sink,
|
|
705
|
+
request_cancel=request_cancel,
|
|
706
|
+
release=release,
|
|
707
|
+
)
|
|
708
|
+
if not goal_handle.accepted:
|
|
709
|
+
sink.settle(rejected_outcome(action))
|
|
710
|
+
return handle
|
|
711
|
+
|
|
712
|
+
def on_result(future: Any) -> None:
|
|
713
|
+
loop.call_soon_threadsafe(_settle, future)
|
|
714
|
+
|
|
715
|
+
def _settle(future: Any) -> None:
|
|
716
|
+
try:
|
|
717
|
+
response = future.result()
|
|
718
|
+
outcome = settled_outcome(GoalStatus(response.status), decode(response.result, types.result))
|
|
719
|
+
except Exception as error:
|
|
720
|
+
# An unreadable result still ends the goal; report it as an outcome, since this
|
|
721
|
+
# callback has nobody to raise to.
|
|
722
|
+
outcome = GoalOutcome(
|
|
723
|
+
faults=(f"the result of the goal on {action} could not be read: {error!r}",),
|
|
724
|
+
accepted=True,
|
|
725
|
+
status=GoalStatus.UNKNOWN,
|
|
726
|
+
result=None,
|
|
727
|
+
)
|
|
728
|
+
sink.settle(outcome)
|
|
729
|
+
|
|
730
|
+
(await asyncio.to_thread(goal_handle.get_result_async)).add_done_callback(on_result)
|
|
731
|
+
return handle
|
|
732
|
+
|
|
733
|
+
def _destroy_action_client(self, client: Any) -> None:
|
|
734
|
+
"""Destroy one ``rclpy`` action client and wake the executor so its wait set is rebuilt."""
|
|
735
|
+
client.destroy()
|
|
736
|
+
self._wake()
|
|
737
|
+
|
|
738
|
+
async def _read_graph(self) -> GraphSnapshot:
|
|
739
|
+
"""Read every name on the graph once, off the loop; this is what :attr:`graph` polls with.
|
|
740
|
+
|
|
741
|
+
:raises RuntimeNotStartedError: When the Runtime is not up (a poll racing teardown).
|
|
742
|
+
"""
|
|
743
|
+
node = self._require_started()
|
|
744
|
+
return await asyncio.to_thread(_snapshot_of, node)
|
|
745
|
+
|
|
746
|
+
async def _call_parameter_service(
|
|
747
|
+
self,
|
|
748
|
+
node: str,
|
|
749
|
+
service: str,
|
|
750
|
+
service_type: str,
|
|
751
|
+
build: Callable[[type[Message]], Message],
|
|
752
|
+
) -> Message:
|
|
753
|
+
"""Call one parameter service of a ROS2 node.
|
|
754
|
+
|
|
755
|
+
:raises ServiceUnavailableError: When the node's service does not answer in time.
|
|
756
|
+
:raises ServiceCallTimeoutError: When the request went and no answer came back.
|
|
757
|
+
"""
|
|
758
|
+
types = self.describe_service(service_type)
|
|
759
|
+
return await self.call_service(
|
|
760
|
+
service=f"{node}/{service}",
|
|
761
|
+
service_type=types,
|
|
762
|
+
request=build(types.request),
|
|
763
|
+
server_timeout=SERVER_TIMEOUT,
|
|
764
|
+
call_timeout=CALL_TIMEOUT,
|
|
765
|
+
)
|
|
766
|
+
|
|
767
|
+
async def _read_parameter(self, node: str, name: str) -> ParameterReading:
|
|
768
|
+
"""Read one parameter of one ROS2 node (one name per call, see ``ParameterFacet``)."""
|
|
769
|
+
response = await self._call_parameter_service(
|
|
770
|
+
node, "get_parameters", GET_PARAMETERS_TYPE, lambda request: get_request(request, name)
|
|
771
|
+
)
|
|
772
|
+
return reading_of(name, response)
|
|
773
|
+
|
|
774
|
+
async def _list_parameters(self, node: str) -> tuple[str, ...]:
|
|
775
|
+
"""Ask one ROS2 node what parameters it declares, recursively."""
|
|
776
|
+
return names_of(await self._call_parameter_service(node, "list_parameters", LIST_PARAMETERS_TYPE, list_request))
|
|
777
|
+
|
|
778
|
+
async def _describe_parameter(self, node: str, name: str) -> ParameterDescription:
|
|
779
|
+
"""Ask one ROS2 node about one parameter (one name per call: a batch with an undeclared name answers empty)."""
|
|
780
|
+
response = await self._call_parameter_service(
|
|
781
|
+
node, "describe_parameters", DESCRIBE_PARAMETERS_TYPE, lambda request: describe_request(request, name)
|
|
782
|
+
)
|
|
783
|
+
return description_of(name, response)
|
|
784
|
+
|
|
785
|
+
async def _write_parameters(self, node: str, values: Mapping[str, ParameterValue]) -> Mapping[str, SetOutcome]:
|
|
786
|
+
"""Write a batch of parameters to one ROS2 node, with one outcome per parameter.
|
|
787
|
+
|
|
788
|
+
Uses ``set_parameters``, not the atomic variant, which discards the whole batch when one
|
|
789
|
+
name is undeclared.
|
|
790
|
+
"""
|
|
791
|
+
response = await self._call_parameter_service(
|
|
792
|
+
node, "set_parameters", SET_PARAMETERS_TYPE, lambda request: set_request(request, values)
|
|
793
|
+
)
|
|
794
|
+
return outcomes_of(tuple(values), response)
|
|
795
|
+
|
|
796
|
+
async def _follow_parameter_events(self) -> AsyncGenerator[ParameterUpdate, None]:
|
|
797
|
+
"""Yield every parameter change on the graph from one ``/parameter_events`` subscription.
|
|
798
|
+
|
|
799
|
+
The facet demultiplexes by node. The subscription closes with the generator.
|
|
800
|
+
"""
|
|
801
|
+
subscription = await self.subscribe(
|
|
802
|
+
topic=PARAMETER_EVENTS_TOPIC,
|
|
803
|
+
message_type=PARAMETER_EVENT_TYPE,
|
|
804
|
+
depth=EVENT_DEPTH,
|
|
805
|
+
)
|
|
806
|
+
try:
|
|
807
|
+
async for message in subscription:
|
|
808
|
+
yield update_of(message)
|
|
809
|
+
finally:
|
|
810
|
+
await subscription.close()
|
|
811
|
+
|
|
812
|
+
def _destroy(self, kind: str, endpoint: Any) -> None:
|
|
813
|
+
"""Destroy one ``rclpy`` subscription, publisher or client, and wake the executor."""
|
|
814
|
+
if self._node is not None:
|
|
815
|
+
getattr(self._node, f"destroy_{kind}")(endpoint)
|
|
816
|
+
self._wake()
|
|
817
|
+
|
|
818
|
+
def _wake(self) -> None:
|
|
819
|
+
"""Wake the executor so its wait set is rebuilt after an endpoint went away."""
|
|
820
|
+
if self._executor is not None:
|
|
821
|
+
self._executor.wake()
|
|
822
|
+
|
|
823
|
+
def _report_type_hash_mismatches(self, topic: str, sink: SubscriptionSink) -> None:
|
|
824
|
+
"""Compare every publisher's type hash against our own, and warn about each new one.
|
|
825
|
+
|
|
826
|
+
A warning, not a refusal: a changed hash does not mean the message is undecodable.
|
|
827
|
+
"""
|
|
828
|
+
node = self._node
|
|
829
|
+
if node is None:
|
|
830
|
+
return
|
|
831
|
+
expected = _own_type_hash(node, topic)
|
|
832
|
+
if expected is None:
|
|
833
|
+
return
|
|
834
|
+
for info in node.get_publishers_info_by_topic(topic):
|
|
835
|
+
published = str(info.topic_type_hash)
|
|
836
|
+
if published in (_UNKNOWN_TYPE_HASH, expected):
|
|
837
|
+
continue
|
|
838
|
+
mismatch = TypeHashMismatch(
|
|
839
|
+
topic=topic,
|
|
840
|
+
node_name=f"{info.node_namespace.rstrip('/')}/{info.node_name}",
|
|
841
|
+
topic_type=info.topic_type,
|
|
842
|
+
published_hash=published,
|
|
843
|
+
expected_hash=expected,
|
|
844
|
+
)
|
|
845
|
+
sink.note_type_hash_mismatch(mismatch)
|
|
846
|
+
warnings.warn(
|
|
847
|
+
f"{mismatch.node_name} publishes {mismatch.topic_type} on {topic} with type hash "
|
|
848
|
+
f"{published}, but this adapter was built against {expected}",
|
|
849
|
+
TypeHashMismatchWarning,
|
|
850
|
+
stacklevel=2,
|
|
851
|
+
)
|
|
852
|
+
|
|
853
|
+
def _require_started(self) -> rclpy.node.Node:
|
|
854
|
+
"""Return the node, or refuse because there is none."""
|
|
855
|
+
if self._node is None:
|
|
856
|
+
message = f"{self._node_name} is not started; call start() before using the Runtime"
|
|
857
|
+
raise RuntimeNotStartedError(message)
|
|
858
|
+
return self._node
|
|
859
|
+
|
|
860
|
+
def _require_loop(self) -> asyncio.AbstractEventLoop:
|
|
861
|
+
"""Return the Runtime's loop, or refuse because this is not it."""
|
|
862
|
+
loop = self._loop
|
|
863
|
+
if loop is None:
|
|
864
|
+
message = f"{self._node_name} is not started; call start() before using the Runtime"
|
|
865
|
+
raise RuntimeNotStartedError(message)
|
|
866
|
+
if loop is not asyncio.get_running_loop():
|
|
867
|
+
message = (
|
|
868
|
+
f"{self._node_name} is affine to the event loop it was started on; this call is "
|
|
869
|
+
f"on another one, which would receive none of its messages"
|
|
870
|
+
)
|
|
871
|
+
raise LoopAffinityError(message)
|
|
872
|
+
return loop
|
|
873
|
+
|
|
874
|
+
|
|
875
|
+
@final
|
|
876
|
+
class _AdaptiveSubscription:
|
|
877
|
+
"""One topic's QoS negotiation, and the ``rclpy`` subscription it is currently expressed as.
|
|
878
|
+
|
|
879
|
+
The two event methods run on the Runtime's loop and decide synchronously, before any ``await``,
|
|
880
|
+
whether to swap, so a second match event cannot spend the same bootstrap tightening twice. The
|
|
881
|
+
swap itself is a task because ``rclpy`` calls block.
|
|
882
|
+
"""
|
|
883
|
+
|
|
884
|
+
def __init__(
|
|
885
|
+
self,
|
|
886
|
+
*,
|
|
887
|
+
topic: str,
|
|
888
|
+
negotiation: QoSNegotiation,
|
|
889
|
+
sink: SubscriptionSink,
|
|
890
|
+
loop: asyncio.AbstractEventLoop,
|
|
891
|
+
create: Callable[[QoSProfile], Awaitable[Any]],
|
|
892
|
+
destroy: Callable[[Any], Awaitable[None]],
|
|
893
|
+
publishers: Callable[[], tuple[EndpointInfo, ...]],
|
|
894
|
+
) -> None:
|
|
895
|
+
"""Bind one negotiation to the two ``rclpy`` steps and the graph read the Runtime owns."""
|
|
896
|
+
self._topic = topic
|
|
897
|
+
self._negotiation = negotiation
|
|
898
|
+
self._sink = sink
|
|
899
|
+
self._loop = loop
|
|
900
|
+
self._create = create
|
|
901
|
+
self._destroy = destroy
|
|
902
|
+
self._publishers = publishers
|
|
903
|
+
self._subscription: Any = None
|
|
904
|
+
self._swapping = asyncio.Lock()
|
|
905
|
+
self._tasks: set[asyncio.Task[None]] = set()
|
|
906
|
+
self._closed = False
|
|
907
|
+
|
|
908
|
+
async def open(self) -> None:
|
|
909
|
+
"""Create the first ``rclpy`` subscription, with whatever the negotiation opened at."""
|
|
910
|
+
self._subscription = await self._create(self._negotiation.profile)
|
|
911
|
+
|
|
912
|
+
def matched(self, count: int) -> None:
|
|
913
|
+
"""Record the publisher count, and spend the one bootstrap tightening when it is due."""
|
|
914
|
+
self._sink.note_publishers(count)
|
|
915
|
+
if count <= 0:
|
|
916
|
+
return
|
|
917
|
+
wanted = self._negotiation.on_first_publisher(self._publishers())
|
|
918
|
+
if wanted is None:
|
|
919
|
+
return
|
|
920
|
+
_LOGGER.info(
|
|
921
|
+
"Re-resolving a discovered QoS profile against the first publisher on the topic",
|
|
922
|
+
topic=self._topic,
|
|
923
|
+
profile=str(wanted),
|
|
924
|
+
)
|
|
925
|
+
self._swap(wanted)
|
|
926
|
+
|
|
927
|
+
def incompatible(self, policy: Policy) -> None:
|
|
928
|
+
"""Report one incompatibility, and relax only if this profile was discovered."""
|
|
929
|
+
reported = self._negotiation.incompatibility(self._topic, policy, self._publishers())
|
|
930
|
+
if not self._sink.note_incompatibility(reported):
|
|
931
|
+
# Already reported and unchanged: a flapping publisher must not cause a log storm.
|
|
932
|
+
return
|
|
933
|
+
_LOGGER.error(
|
|
934
|
+
"A publisher offers QoS this subscription cannot accept",
|
|
935
|
+
topic=self._topic,
|
|
936
|
+
policy=policy.name,
|
|
937
|
+
provenance=reported.provenance.value,
|
|
938
|
+
requested=str(reported.request),
|
|
939
|
+
adapted=str(reported.adapted) if reported.adapted is not None else None,
|
|
940
|
+
)
|
|
941
|
+
if reported.adapted is not None:
|
|
942
|
+
self._swap(reported.adapted)
|
|
943
|
+
|
|
944
|
+
def _swap(self, profile: QoSProfile) -> None:
|
|
945
|
+
"""Start the re-subscription as a task that :meth:`close` can wait for."""
|
|
946
|
+
if self._closed:
|
|
947
|
+
return
|
|
948
|
+
task = self._loop.create_task(
|
|
949
|
+
self._resubscribe(profile),
|
|
950
|
+
name=f"ros2-qos-swap{self._topic}",
|
|
951
|
+
)
|
|
952
|
+
self._tasks.add(task)
|
|
953
|
+
task.add_done_callback(self._tasks.discard)
|
|
954
|
+
|
|
955
|
+
async def _resubscribe(self, profile: QoSProfile) -> None:
|
|
956
|
+
"""Destroy the current ``rclpy`` subscription and create one asking for ``profile``.
|
|
957
|
+
|
|
958
|
+
The old subscription goes first, so two never report match events into one sink; the cost
|
|
959
|
+
is a few milliseconds of missed messages. The publisher count goes back to zero and is
|
|
960
|
+
rebuilt from the new subscription's match events, because destroying a subscription
|
|
961
|
+
discards its match state without an event; a carried-over count could leave a variable
|
|
962
|
+
reading ``Good`` after its publisher went away, at the price of a momentary
|
|
963
|
+
``BadNoCommunication``.
|
|
964
|
+
"""
|
|
965
|
+
async with self._swapping:
|
|
966
|
+
if self._closed:
|
|
967
|
+
return
|
|
968
|
+
held = self._sink.qos
|
|
969
|
+
current, self._subscription = self._subscription, None
|
|
970
|
+
if current is not None:
|
|
971
|
+
await self._destroy(current)
|
|
972
|
+
self._sink.note_publishers(0)
|
|
973
|
+
if await self._recreate(profile):
|
|
974
|
+
self._sink.note_qos(profile)
|
|
975
|
+
elif not await self._recreate(held):
|
|
976
|
+
# Both profiles failed. Nobody to raise to on this task, so log; the zeroed
|
|
977
|
+
# count leaves the variables reading `BadNoCommunication`, not a stale `Good`.
|
|
978
|
+
_LOGGER.error(
|
|
979
|
+
"Could not re-subscribe after a QoS change, on either profile; this topic is "
|
|
980
|
+
"now unsubscribed and its variables read BadNoCommunication",
|
|
981
|
+
topic=self._topic,
|
|
982
|
+
wanted=str(profile),
|
|
983
|
+
held=str(held),
|
|
984
|
+
)
|
|
985
|
+
|
|
986
|
+
async def _recreate(self, profile: QoSProfile) -> bool:
|
|
987
|
+
"""Try to create the ``rclpy`` subscription with one profile, saying whether it worked."""
|
|
988
|
+
try:
|
|
989
|
+
self._subscription = await self._create(profile)
|
|
990
|
+
except Exception:
|
|
991
|
+
_LOGGER.exception(
|
|
992
|
+
"Could not create a subscription with this QoS profile",
|
|
993
|
+
topic=self._topic,
|
|
994
|
+
profile=str(profile),
|
|
995
|
+
)
|
|
996
|
+
return False
|
|
997
|
+
return True
|
|
998
|
+
|
|
999
|
+
async def close(self) -> None:
|
|
1000
|
+
"""Let any swap in flight finish, then destroy the ``rclpy`` subscription, once.
|
|
1001
|
+
|
|
1002
|
+
A swap in flight is waited out, not cancelled: cancelling an ``asyncio.to_thread`` call
|
|
1003
|
+
stops the await but not the thread, which would leave a subscription nobody holds.
|
|
1004
|
+
"""
|
|
1005
|
+
self._closed = True
|
|
1006
|
+
async with self._swapping:
|
|
1007
|
+
current, self._subscription = self._subscription, None
|
|
1008
|
+
for task in tuple(self._tasks):
|
|
1009
|
+
await asyncio.gather(task, return_exceptions=True)
|
|
1010
|
+
if current is not None:
|
|
1011
|
+
await self._destroy(current)
|
|
1012
|
+
|
|
1013
|
+
|
|
1014
|
+
_INFINITE_NANOSECONDS: Final = 9223372036854775807
|
|
1015
|
+
"""How a live Jazzy graph reports an infinite duration (``INT64_MAX``); a fresh profile says zero. Both mean FOREVER."""
|
|
1016
|
+
|
|
1017
|
+
_RELIABILITY: Final = {
|
|
1018
|
+
Reliability.RELIABLE: rclpy.qos.ReliabilityPolicy.RELIABLE,
|
|
1019
|
+
Reliability.BEST_EFFORT: rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
|
|
1020
|
+
}
|
|
1021
|
+
_DURABILITY: Final = {
|
|
1022
|
+
Durability.TRANSIENT_LOCAL: rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
|
|
1023
|
+
Durability.VOLATILE: rclpy.qos.DurabilityPolicy.VOLATILE,
|
|
1024
|
+
}
|
|
1025
|
+
_LIVELINESS: Final = {
|
|
1026
|
+
Liveliness.MANUAL_BY_TOPIC: rclpy.qos.LivelinessPolicy.MANUAL_BY_TOPIC,
|
|
1027
|
+
Liveliness.AUTOMATIC: rclpy.qos.LivelinessPolicy.AUTOMATIC,
|
|
1028
|
+
}
|
|
1029
|
+
"""This package's policy values in ``rclpy``'s vocabulary (``SYSTEM_DEFAULT`` is never requested)."""
|
|
1030
|
+
|
|
1031
|
+
_RELIABILITY_IN: Final = {int(theirs): ours for ours, theirs in _RELIABILITY.items()}
|
|
1032
|
+
_DURABILITY_IN: Final = {int(theirs): ours for ours, theirs in _DURABILITY.items()}
|
|
1033
|
+
_LIVELINESS_IN: Final = {int(theirs): ours for ours, theirs in _LIVELINESS.items()}
|
|
1034
|
+
"""The same read backwards, by the integer a publisher reports.
|
|
1035
|
+
|
|
1036
|
+
``SYSTEM_DEFAULT``, ``UNKNOWN`` and ``BEST_AVAILABLE`` are absent and fall through to the most
|
|
1037
|
+
permissive value, so an offer we cannot interpret never makes us ask for more than it.
|
|
1038
|
+
"""
|
|
1039
|
+
|
|
1040
|
+
|
|
1041
|
+
def _rclpy_profile(profile: QoSProfile, depth: int) -> rclpy.qos.QoSProfile:
|
|
1042
|
+
"""Return one of our profiles as the ``rclpy`` object an endpoint is created with, joined with its depth."""
|
|
1043
|
+
return rclpy.qos.QoSProfile(
|
|
1044
|
+
depth=depth,
|
|
1045
|
+
history=rclpy.qos.HistoryPolicy.KEEP_LAST,
|
|
1046
|
+
reliability=_RELIABILITY[profile.reliability],
|
|
1047
|
+
durability=_DURABILITY[profile.durability],
|
|
1048
|
+
deadline=_duration_of(profile.deadline),
|
|
1049
|
+
liveliness=_LIVELINESS[profile.liveliness],
|
|
1050
|
+
liveliness_lease_duration=_duration_of(profile.lease),
|
|
1051
|
+
)
|
|
1052
|
+
|
|
1053
|
+
|
|
1054
|
+
def _our_profile(profile: Any) -> QoSProfile:
|
|
1055
|
+
"""Return what one endpoint offers; anything not modelled reads as the most permissive value."""
|
|
1056
|
+
return QoSProfile(
|
|
1057
|
+
reliability=_RELIABILITY_IN.get(int(profile.reliability), Reliability.BEST_EFFORT),
|
|
1058
|
+
durability=_DURABILITY_IN.get(int(profile.durability), Durability.VOLATILE),
|
|
1059
|
+
deadline=_seconds_of(profile.deadline),
|
|
1060
|
+
liveliness=_LIVELINESS_IN.get(int(profile.liveliness), Liveliness.AUTOMATIC),
|
|
1061
|
+
lease=_seconds_of(profile.liveliness_lease_duration),
|
|
1062
|
+
)
|
|
1063
|
+
|
|
1064
|
+
|
|
1065
|
+
def _duration_of(seconds: float) -> Duration:
|
|
1066
|
+
"""Return a number of seconds as the ``rclpy`` duration a QoS policy takes."""
|
|
1067
|
+
if seconds == FOREVER:
|
|
1068
|
+
return Duration(nanoseconds=_INFINITE_NANOSECONDS)
|
|
1069
|
+
return Duration(nanoseconds=int(seconds * 1_000_000_000))
|
|
1070
|
+
|
|
1071
|
+
|
|
1072
|
+
def _seconds_of(duration: Any) -> float:
|
|
1073
|
+
"""Return an ``rclpy`` duration as seconds, with both spellings of *no duration* as FOREVER."""
|
|
1074
|
+
nanoseconds = int(duration.nanoseconds)
|
|
1075
|
+
if nanoseconds <= 0 or nanoseconds >= _INFINITE_NANOSECONDS:
|
|
1076
|
+
return FOREVER
|
|
1077
|
+
return nanoseconds / 1_000_000_000
|
|
1078
|
+
|
|
1079
|
+
|
|
1080
|
+
def _policy_of(kind: Any) -> Policy:
|
|
1081
|
+
"""Return the policy an incompatibility event named; an unmodelled kind is ``Policy.INVALID``, not an exception."""
|
|
1082
|
+
try:
|
|
1083
|
+
return Policy(int(kind))
|
|
1084
|
+
except ValueError:
|
|
1085
|
+
return Policy.INVALID
|
|
1086
|
+
|
|
1087
|
+
|
|
1088
|
+
def _snapshot_of(node: rclpy.node.Node) -> GraphSnapshot:
|
|
1089
|
+
"""Return every topic, service and action on the graph, with the types on each name.
|
|
1090
|
+
|
|
1091
|
+
A topic only this node subscribes to is included; match events, not this poll, say whether
|
|
1092
|
+
anything publishes on it.
|
|
1093
|
+
"""
|
|
1094
|
+
return GraphSnapshot(
|
|
1095
|
+
topics=_by_name(node.get_topic_names_and_types()),
|
|
1096
|
+
services=_by_name(node.get_service_names_and_types()),
|
|
1097
|
+
actions=_by_name(get_action_names_and_types(node)),
|
|
1098
|
+
)
|
|
1099
|
+
|
|
1100
|
+
|
|
1101
|
+
def _by_name(listed: Iterable[tuple[str, list[str]]]) -> dict[str, tuple[str, ...]]:
|
|
1102
|
+
"""Return one of rclpy's name-and-types lists as the mapping a snapshot holds."""
|
|
1103
|
+
return {name: tuple(types) for name, types in listed}
|
|
1104
|
+
|
|
1105
|
+
|
|
1106
|
+
def _own_type_hash(node: rclpy.node.Node, topic: str) -> str | None:
|
|
1107
|
+
"""Return the type hash of this node's own subscription on a topic (what the dataclass was built against)."""
|
|
1108
|
+
ours = node.get_name(), node.get_namespace()
|
|
1109
|
+
for info in node.get_subscriptions_info_by_topic(topic):
|
|
1110
|
+
if (info.node_name, info.node_namespace) == ours:
|
|
1111
|
+
hash_text = str(info.topic_type_hash)
|
|
1112
|
+
return None if hash_text == _UNKNOWN_TYPE_HASH else hash_text
|
|
1113
|
+
return None
|
|
1114
|
+
|
|
1115
|
+
|
|
1116
|
+
def _require_the_payload_type(
|
|
1117
|
+
what: str,
|
|
1118
|
+
path: str,
|
|
1119
|
+
payload: Message,
|
|
1120
|
+
expected: type[Message],
|
|
1121
|
+
type_name: str,
|
|
1122
|
+
) -> None:
|
|
1123
|
+
"""Refuse a payload that is not the message dataclass its ROS2 path takes.
|
|
1124
|
+
|
|
1125
|
+
:raises TypeError: When the payload is not an instance of the expected dataclass.
|
|
1126
|
+
"""
|
|
1127
|
+
if isinstance(payload, expected):
|
|
1128
|
+
return
|
|
1129
|
+
message = f"the {what} for {path} is a {type(payload).__name__}, and {type_name} takes a {expected.__name__}"
|
|
1130
|
+
raise TypeError(message)
|
|
1131
|
+
|
|
1132
|
+
|
|
1133
|
+
def _refuse_an_unserved_action(action: str, server_timeout: float) -> None:
|
|
1134
|
+
"""Refuse a goal for a path no action server answers on."""
|
|
1135
|
+
message = f"no action server answered on {action} within {server_timeout} seconds, so the goal was not sent"
|
|
1136
|
+
raise ActionServerUnavailableError(message)
|
|
1137
|
+
|
|
1138
|
+
|
|
1139
|
+
def _refuse_an_unserved_service(service: str, server_timeout: float) -> None:
|
|
1140
|
+
"""Refuse a call on a path no service server answers on."""
|
|
1141
|
+
message = f"no service server answered on {service} within {server_timeout} seconds, so the request was not sent"
|
|
1142
|
+
raise ServiceUnavailableError(message)
|
|
1143
|
+
|
|
1144
|
+
|
|
1145
|
+
async def _resolved(future: Any, loop: asyncio.AbstractEventLoop) -> Any:
|
|
1146
|
+
"""Await an ``rclpy`` future (not awaitable, completes on the executor thread) on the Runtime's loop."""
|
|
1147
|
+
waiter: asyncio.Future[Any] = loop.create_future()
|
|
1148
|
+
|
|
1149
|
+
def on_done(done: Any) -> None:
|
|
1150
|
+
loop.call_soon_threadsafe(_settle, done)
|
|
1151
|
+
|
|
1152
|
+
def _settle(done: Any) -> None:
|
|
1153
|
+
# The waiter was cancelled (e.g. a timed-out service call) and the future resolved later.
|
|
1154
|
+
if waiter.done():
|
|
1155
|
+
return
|
|
1156
|
+
try:
|
|
1157
|
+
waiter.set_result(done.result())
|
|
1158
|
+
except Exception as error:
|
|
1159
|
+
waiter.set_exception(error)
|
|
1160
|
+
|
|
1161
|
+
future.add_done_callback(on_done)
|
|
1162
|
+
return await waiter
|
|
1163
|
+
|
|
1164
|
+
|
|
1165
|
+
def _faults_from(what: str, step: Callable[[], object]) -> Iterable[str]:
|
|
1166
|
+
"""Run one teardown step, returning a fault line instead of raising, so a failed step cannot keep the Context up."""
|
|
1167
|
+
try:
|
|
1168
|
+
step()
|
|
1169
|
+
except Exception as error:
|
|
1170
|
+
return [f"{what} failed: {error!r}"]
|
|
1171
|
+
return []
|