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,714 @@
|
|
|
1
|
+
# SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
|
|
2
|
+
#
|
|
3
|
+
# SPDX-License-Identifier: MIT
|
|
4
|
+
|
|
5
|
+
"""The TF facet: both transform topics, held by us, in both directions.
|
|
6
|
+
|
|
7
|
+
TF is ROS2's frame network: coordinate frames, each naming its parent, carried on ``/tf`` (moving)
|
|
8
|
+
and ``/tf_static`` (never moving). Inward, the facet drains both topics through two ordinary
|
|
9
|
+
subscriptions into a topology record and a ``tf2_ros.Buffer``. It does not use
|
|
10
|
+
``tf2_ros.TransformListener``, which would start a second executor and break the rule that this
|
|
11
|
+
package owns one Node and one spinning thread. The topology is read from the messages (parent
|
|
12
|
+
``header.frame_id``, child ``child_frame_id``), geometry is looked up synchronously with
|
|
13
|
+
``Buffer.lookup_transform`` and no timeout (its ``_async`` variant busy-spins on the Runtime's
|
|
14
|
+
loop), and nothing here imports ``rclpy``.
|
|
15
|
+
|
|
16
|
+
Outward, :meth:`TFFacet.broadcast` publishes on the same topics. The publishers are opened by a
|
|
17
|
+
watch that declares it broadcasts, so a Machine without outward frames never advertises ``/tf``. Our
|
|
18
|
+
own publisher is discounted from :attr:`TFFacet.satisfied`, since a subscription's matched count
|
|
19
|
+
includes it (checked on Jazzy). The stamp is one reading of the Runtime's clock per call
|
|
20
|
+
(:mod:`opensmi.ros2.runtime.stamp`). ``tf2_ros.StaticTransformBroadcaster`` is not used, because it
|
|
21
|
+
silently drops an update for a frame it already sent. The static publisher is transient-local and
|
|
22
|
+
the moving one volatile (:data:`TF_STATIC_QOS`, :data:`TF_QOS`); both are declared constants, while
|
|
23
|
+
the subscriptions are discovered.
|
|
24
|
+
"""
|
|
25
|
+
|
|
26
|
+
from __future__ import annotations
|
|
27
|
+
|
|
28
|
+
import asyncio
|
|
29
|
+
from dataclasses import dataclass
|
|
30
|
+
from typing import TYPE_CHECKING, Any, Final, TypeAlias, final
|
|
31
|
+
|
|
32
|
+
import structlog
|
|
33
|
+
|
|
34
|
+
from opensmi.ros2.runtime.errors import (
|
|
35
|
+
ROS2RuntimeError,
|
|
36
|
+
UnusableMessageTypeError,
|
|
37
|
+
)
|
|
38
|
+
from opensmi.ros2.runtime.messages import repeated_message
|
|
39
|
+
from opensmi.ros2.runtime.qos import DEFAULT_PROFILE, LATCHED
|
|
40
|
+
from opensmi.ros2.runtime.stamp import NANOSECONDS_FIELD, SECONDS_FIELD, STAMP_FIELD
|
|
41
|
+
|
|
42
|
+
if TYPE_CHECKING:
|
|
43
|
+
from collections.abc import Awaitable, Callable, Iterable, Mapping, Sequence
|
|
44
|
+
|
|
45
|
+
from opensmi.ros2.runtime.messages import Message
|
|
46
|
+
from opensmi.ros2.runtime.publisher import Publisher
|
|
47
|
+
from opensmi.ros2.runtime.subscription import Subscription
|
|
48
|
+
|
|
49
|
+
__all__ = [
|
|
50
|
+
"CHILD_FRAME_FIELD",
|
|
51
|
+
"FRAME_FIELD",
|
|
52
|
+
"HEADER_FIELD",
|
|
53
|
+
"ROTATION_FIELD",
|
|
54
|
+
"TF_DEPTH",
|
|
55
|
+
"TF_MESSAGE_TYPE",
|
|
56
|
+
"TF_QOS",
|
|
57
|
+
"TF_STATIC_QOS",
|
|
58
|
+
"TF_STATIC_TOPIC",
|
|
59
|
+
"TF_TOPIC",
|
|
60
|
+
"TRANSFORMS_FIELD",
|
|
61
|
+
"TRANSFORM_CACHE",
|
|
62
|
+
"TRANSFORM_FIELD",
|
|
63
|
+
"TRANSLATION_FIELD",
|
|
64
|
+
"AdvertiseFunction",
|
|
65
|
+
"ClockFunction",
|
|
66
|
+
"FeedFunction",
|
|
67
|
+
"FrameLink",
|
|
68
|
+
"LookupFunction",
|
|
69
|
+
"SubscribeFunction",
|
|
70
|
+
"TFFacet",
|
|
71
|
+
"TFWatch",
|
|
72
|
+
"Transform",
|
|
73
|
+
"links",
|
|
74
|
+
"roots_of",
|
|
75
|
+
]
|
|
76
|
+
|
|
77
|
+
TF_TOPIC: Final = "/tf"
|
|
78
|
+
"""The topic every moving transform in a ROS2 system is published on."""
|
|
79
|
+
|
|
80
|
+
TF_STATIC_TOPIC: Final = "/tf_static"
|
|
81
|
+
"""The topic a transform that never moves is published on, once, with transient-local QoS."""
|
|
82
|
+
|
|
83
|
+
TF_MESSAGE_TYPE: Final = "tf2_msgs/msg/TFMessage"
|
|
84
|
+
"""The one type both topics carry."""
|
|
85
|
+
|
|
86
|
+
TF_QOS: Final = DEFAULT_PROFILE
|
|
87
|
+
"""What this Runtime offers on the moving transform topic: reliable and volatile.
|
|
88
|
+
|
|
89
|
+
Replaying an old pose to a subscriber that has just joined would report a robot where it used to be.
|
|
90
|
+
"""
|
|
91
|
+
|
|
92
|
+
TF_STATIC_QOS: Final = LATCHED
|
|
93
|
+
"""What this Runtime offers on the static transform topic: reliable and transient-local.
|
|
94
|
+
|
|
95
|
+
This is the ROS2 convention. A static frame is published once, so a volatile publisher would leave
|
|
96
|
+
every later subscriber with no frame. It is a constant, because ``tf2_ros``' own static broadcaster
|
|
97
|
+
hard-codes the same profile.
|
|
98
|
+
"""
|
|
99
|
+
|
|
100
|
+
TRANSFORMS_FIELD: Final = "transforms"
|
|
101
|
+
"""The repeated field of a ``TFMessage``, which is what keys the frame network."""
|
|
102
|
+
|
|
103
|
+
CHILD_FRAME_FIELD: Final = "child_frame_id"
|
|
104
|
+
"""The field of one element naming the frame it describes. The parent is in its header."""
|
|
105
|
+
|
|
106
|
+
HEADER_FIELD: Final = "header"
|
|
107
|
+
"""The field of one element carrying the parent frame and the stamp. Read inward, written outward."""
|
|
108
|
+
|
|
109
|
+
TRANSFORM_FIELD: Final = "transform"
|
|
110
|
+
"""The field of one element carrying the geometry: a translation and a rotation."""
|
|
111
|
+
|
|
112
|
+
TRANSLATION_FIELD: Final = "translation"
|
|
113
|
+
"""The field of one transform holding metres along three axes."""
|
|
114
|
+
|
|
115
|
+
ROTATION_FIELD: Final = "rotation"
|
|
116
|
+
"""The field of one transform holding the unit quaternion."""
|
|
117
|
+
|
|
118
|
+
FRAME_FIELD: Final = "frame_id"
|
|
119
|
+
"""The field of one header naming the parent frame. Read inward, written outward."""
|
|
120
|
+
|
|
121
|
+
TRANSFORM_CACHE: Final = 10.0
|
|
122
|
+
"""How many seconds of transform history the buffer keeps (tf2's default). Composed poses are answered from it."""
|
|
123
|
+
|
|
124
|
+
TF_DEPTH: Final = 100
|
|
125
|
+
"""How many messages each transform subscription buffers.
|
|
126
|
+
|
|
127
|
+
Larger than the Binding default, because ``/tf`` carries one message per publisher per control
|
|
128
|
+
cycle. A drop here is a transform the buffer never saw.
|
|
129
|
+
"""
|
|
130
|
+
|
|
131
|
+
_LOGGER = structlog.get_logger("opensmi.ros2.runtime.tf")
|
|
132
|
+
|
|
133
|
+
SubscribeFunction: TypeAlias = "Callable[..., Awaitable[Subscription]]"
|
|
134
|
+
"""How the facet opens a topic: the Runtime's own ``subscribe``."""
|
|
135
|
+
|
|
136
|
+
AdvertiseFunction: TypeAlias = "Callable[..., Awaitable[Publisher]]"
|
|
137
|
+
"""How the facet opens a topic outward: the Runtime's own ``advertise``."""
|
|
138
|
+
|
|
139
|
+
ClockFunction: TypeAlias = "Callable[[], tuple[int, int]]"
|
|
140
|
+
"""How a broadcast learns what time it is: whole seconds and nanoseconds off the ROS2 Node's own clock.
|
|
141
|
+
|
|
142
|
+
It sits behind the seam because a Node on simulated time is the case a wall clock gets wrong.
|
|
143
|
+
"""
|
|
144
|
+
|
|
145
|
+
FeedFunction: TypeAlias = "Callable[[Message, bool], None]"
|
|
146
|
+
"""How one message reaches the transform buffer: the message, and whether it arrived static."""
|
|
147
|
+
|
|
148
|
+
LookupFunction: TypeAlias = "Callable[[str, str], Transform | None]"
|
|
149
|
+
"""How a composed pose is asked for: a target frame and a source frame.
|
|
150
|
+
|
|
151
|
+
The answer comes from the cache or not at all. Synchronous and non-blocking.
|
|
152
|
+
"""
|
|
153
|
+
|
|
154
|
+
|
|
155
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
156
|
+
class Transform:
|
|
157
|
+
"""One rigid-body transform, as ROS2 spells it: a translation and a unit quaternion.
|
|
158
|
+
|
|
159
|
+
It is the ROS2 shape because this is behind the seam. Turning the quaternion into angles is the
|
|
160
|
+
server half's job.
|
|
161
|
+
|
|
162
|
+
:ivar translation: Metres along x, y and z.
|
|
163
|
+
:ivar rotation: The quaternion as x, y, z, w (ROS2's own field order).
|
|
164
|
+
"""
|
|
165
|
+
|
|
166
|
+
translation: tuple[float, float, float]
|
|
167
|
+
rotation: tuple[float, float, float, float]
|
|
168
|
+
|
|
169
|
+
|
|
170
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
171
|
+
class FrameLink:
|
|
172
|
+
"""One link of the frame network, whichever direction it is travelling.
|
|
173
|
+
|
|
174
|
+
Inward it is what the last message to carry a frame left it as. Outward it is what
|
|
175
|
+
:meth:`TFFacet.broadcast` is asked to put on the wire.
|
|
176
|
+
|
|
177
|
+
:ivar child: The frame this link describes.
|
|
178
|
+
:ivar parent: The frame it is relative to. tf2 permits one parent per frame, so the network is a
|
|
179
|
+
tree.
|
|
180
|
+
:ivar transform: Where the child sits in its parent.
|
|
181
|
+
:ivar static: Inward, whether it arrived on ``/tf_static``. Outward, which topic it is published
|
|
182
|
+
to.
|
|
183
|
+
"""
|
|
184
|
+
|
|
185
|
+
child: str
|
|
186
|
+
parent: str
|
|
187
|
+
transform: Transform
|
|
188
|
+
static: bool
|
|
189
|
+
|
|
190
|
+
|
|
191
|
+
def links(message: Message, *, static: bool) -> tuple[FrameLink, ...]:
|
|
192
|
+
"""Return the frame links one ``TFMessage`` carries, read off its own structure.
|
|
193
|
+
|
|
194
|
+
An element missing either name, or naming itself as its parent, is skipped: one malformed
|
|
195
|
+
element must not end the stream.
|
|
196
|
+
|
|
197
|
+
:param message: One message from ``/tf`` or ``/tf_static``, as it crossed the seam.
|
|
198
|
+
:param static: Whether it arrived on the static topic.
|
|
199
|
+
"""
|
|
200
|
+
found: list[FrameLink] = []
|
|
201
|
+
for element in getattr(message, TRANSFORMS_FIELD, ()):
|
|
202
|
+
child = _named(getattr(element, CHILD_FRAME_FIELD, None))
|
|
203
|
+
parent = _named(getattr(getattr(element, HEADER_FIELD, None), FRAME_FIELD, None))
|
|
204
|
+
transform = _transform_of(element)
|
|
205
|
+
if not child or not parent or child == parent or transform is None:
|
|
206
|
+
continue
|
|
207
|
+
found.append(FrameLink(child=child, parent=parent, transform=transform, static=static))
|
|
208
|
+
return tuple(found)
|
|
209
|
+
|
|
210
|
+
|
|
211
|
+
def roots_of(frames: Mapping[str, FrameLink]) -> tuple[str, ...]:
|
|
212
|
+
"""Return every frame that is somebody's parent and nobody's child, in sorted order.
|
|
213
|
+
|
|
214
|
+
The root of a TF tree is what OPC 10000-210 calls the World Frame. A well-formed tree has one;
|
|
215
|
+
two means two trees are published into one graph.
|
|
216
|
+
"""
|
|
217
|
+
return tuple(sorted({link.parent for link in frames.values()} - set(frames)))
|
|
218
|
+
|
|
219
|
+
|
|
220
|
+
@final
|
|
221
|
+
class TFWatch:
|
|
222
|
+
"""One consumer's interest in TF, and its lifetime.
|
|
223
|
+
|
|
224
|
+
Handed out by :meth:`TFFacet.watch`. Closing it lets the subscriptions close once no watch is
|
|
225
|
+
left.
|
|
226
|
+
"""
|
|
227
|
+
|
|
228
|
+
def __init__(self, *, facet: TFFacet, release: Callable[[TFWatch], None]) -> None:
|
|
229
|
+
"""Bind a watch to the facet it reads and the deregistration the facet prepared."""
|
|
230
|
+
self._facet = facet
|
|
231
|
+
self._release = release
|
|
232
|
+
self._closed = False
|
|
233
|
+
|
|
234
|
+
def __repr__(self) -> str:
|
|
235
|
+
"""Return a representation naming the watch's state."""
|
|
236
|
+
return f"<TFWatch ({'closed' if self._closed else 'open'})>"
|
|
237
|
+
|
|
238
|
+
@property
|
|
239
|
+
def closed(self) -> bool:
|
|
240
|
+
"""Return whether this watch's lifetime has ended."""
|
|
241
|
+
return self._closed
|
|
242
|
+
|
|
243
|
+
async def close(self) -> None:
|
|
244
|
+
"""End this watch, and with it the subscriptions if it was the last one. Idempotent."""
|
|
245
|
+
if self._closed:
|
|
246
|
+
return
|
|
247
|
+
self._closed = True
|
|
248
|
+
self._release(self)
|
|
249
|
+
await self._facet.settle()
|
|
250
|
+
|
|
251
|
+
|
|
252
|
+
@final
|
|
253
|
+
class TFFacet:
|
|
254
|
+
"""The frame network, for as long as anything is watching it.
|
|
255
|
+
|
|
256
|
+
Construct one through :class:`opensmi.ros2.runtime.runtime.Runtime`. The facet is self-disabling
|
|
257
|
+
like the graph and parameter facets: a Machine with no TF Binding never subscribes to ``/tf``,
|
|
258
|
+
the most expensive topic on a busy robot.
|
|
259
|
+
"""
|
|
260
|
+
|
|
261
|
+
def __init__(
|
|
262
|
+
self,
|
|
263
|
+
*,
|
|
264
|
+
subscribe: SubscribeFunction,
|
|
265
|
+
advertise: AdvertiseFunction,
|
|
266
|
+
feed: FeedFunction,
|
|
267
|
+
lookup: LookupFunction,
|
|
268
|
+
clock: ClockFunction,
|
|
269
|
+
) -> None:
|
|
270
|
+
"""Describe a facet without subscribing to or advertising anything.
|
|
271
|
+
|
|
272
|
+
The topics, and whether it publishes, are the first watch's: they come from a declaration.
|
|
273
|
+
|
|
274
|
+
:param subscribe: How to open a topic inward. The Runtime's own.
|
|
275
|
+
:param advertise: How to open a topic outward. The Runtime's own.
|
|
276
|
+
:param feed: How one message reaches the transform buffer.
|
|
277
|
+
:param lookup: How a composed pose is asked of that buffer.
|
|
278
|
+
:param clock: What time it is, off the ROS2 Node's own clock.
|
|
279
|
+
"""
|
|
280
|
+
self._subscribe = subscribe
|
|
281
|
+
self._advertise = advertise
|
|
282
|
+
self._feed = feed
|
|
283
|
+
self._lookup = lookup
|
|
284
|
+
self._clock = clock
|
|
285
|
+
self._reading: _Sources | None = None
|
|
286
|
+
self._frames: dict[str, FrameLink] = {}
|
|
287
|
+
self._seen: dict[str, float] = {}
|
|
288
|
+
self._clock_started = False
|
|
289
|
+
self._watches: list[TFWatch] = []
|
|
290
|
+
self._sources: list[_Source] = []
|
|
291
|
+
self._outward: dict[bool, Publisher] = {}
|
|
292
|
+
|
|
293
|
+
def __repr__(self) -> str:
|
|
294
|
+
"""Return a representation naming the state and the frame count."""
|
|
295
|
+
state = "reading" if self.reading else "resting"
|
|
296
|
+
return f"<TFFacet {state} over {len(self._frames)} frames>"
|
|
297
|
+
|
|
298
|
+
@property
|
|
299
|
+
def reading(self) -> bool:
|
|
300
|
+
"""Return whether the two subscriptions are open, which they are while a watch is."""
|
|
301
|
+
return bool(self._sources)
|
|
302
|
+
|
|
303
|
+
@property
|
|
304
|
+
def broadcasting(self) -> bool:
|
|
305
|
+
"""Return whether the two publishers are open, which they are while a publishing watch is.
|
|
306
|
+
|
|
307
|
+
``False`` on a Machine that declares no frame of its own.
|
|
308
|
+
"""
|
|
309
|
+
return bool(self._outward)
|
|
310
|
+
|
|
311
|
+
@property
|
|
312
|
+
def frames(self) -> Mapping[str, FrameLink]:
|
|
313
|
+
"""Return the frame network as the messages so far describe it, keyed by child frame.
|
|
314
|
+
|
|
315
|
+
This is a live view of the facet's record, not a copy.
|
|
316
|
+
"""
|
|
317
|
+
return self._frames
|
|
318
|
+
|
|
319
|
+
@property
|
|
320
|
+
def seen(self) -> Mapping[str, float]:
|
|
321
|
+
"""Return when each frame was last in a message, on this Runtime's own clock.
|
|
322
|
+
|
|
323
|
+
A link stays in :attr:`frames` once it has arrived, so this is what tells a frame that is
|
|
324
|
+
published and has not moved from one whose publisher stopped. It is separate from
|
|
325
|
+
:class:`FrameLink` because an outgoing link has no arrival time. Like :attr:`frames`, it is
|
|
326
|
+
a live view.
|
|
327
|
+
"""
|
|
328
|
+
return self._seen
|
|
329
|
+
|
|
330
|
+
@property
|
|
331
|
+
def now(self) -> float:
|
|
332
|
+
"""Return what time it is in seconds, off the same clock a broadcast is stamped from.
|
|
333
|
+
|
|
334
|
+
This is the ROS2 Node's clock and not :func:`time.monotonic`, because replayed data under
|
|
335
|
+
``use_sim_time`` advances the Node's clock and not the wall's. An unset clock reads zero,
|
|
336
|
+
which makes every frame equally old and none stale. When the clock starts, :meth:`_record`
|
|
337
|
+
re-bases earlier stamps so that the robot does not appear to go silent at once.
|
|
338
|
+
"""
|
|
339
|
+
seconds, nanoseconds = self._clock()
|
|
340
|
+
return seconds + nanoseconds / 1e9
|
|
341
|
+
|
|
342
|
+
@property
|
|
343
|
+
def roots(self) -> tuple[str, ...]:
|
|
344
|
+
"""Return every frame that is nobody's child, which is where the tree terminates."""
|
|
345
|
+
return roots_of(self._frames)
|
|
346
|
+
|
|
347
|
+
@property
|
|
348
|
+
def satisfied(self) -> bool:
|
|
349
|
+
"""Return whether anything else is publishing transforms right now.
|
|
350
|
+
|
|
351
|
+
Presence, not data: a publisher that has sent nothing yet still means the robot is on.
|
|
352
|
+
Either topic counts. Our own publisher is discounted, because a subscription's matched count
|
|
353
|
+
includes it (checked on Jazzy) and a required TF Binding would otherwise reach ``Running``
|
|
354
|
+
against a robot that is off.
|
|
355
|
+
"""
|
|
356
|
+
return any(source.subscription.publisher_count > self._ours_on(source.static) for source in self._sources)
|
|
357
|
+
|
|
358
|
+
def _ours_on(self, static: bool) -> int:
|
|
359
|
+
"""Return how many publishers this facet holds on one of the two topics (one or none)."""
|
|
360
|
+
return 1 if static in self._outward else 0
|
|
361
|
+
|
|
362
|
+
def lookup(self, *, target: str, source: str) -> Transform | None:
|
|
363
|
+
"""Return where ``source`` sits in ``target``, or ``None`` when the buffer cannot say.
|
|
364
|
+
|
|
365
|
+
Synchronous and non-blocking: a chain the buffer cannot complete answers ``None`` instead of
|
|
366
|
+
waiting.
|
|
367
|
+
"""
|
|
368
|
+
return self._lookup(target, source)
|
|
369
|
+
|
|
370
|
+
async def watch(
|
|
371
|
+
self,
|
|
372
|
+
*,
|
|
373
|
+
topic: str = TF_TOPIC,
|
|
374
|
+
static_topic: str = TF_STATIC_TOPIC,
|
|
375
|
+
depth: int = TF_DEPTH,
|
|
376
|
+
broadcasting: bool = False,
|
|
377
|
+
) -> TFWatch:
|
|
378
|
+
"""Register interest in TF, opening both subscriptions if they were not open.
|
|
379
|
+
|
|
380
|
+
The first watch decides where TF is read from and every later one must agree. One Runtime is
|
|
381
|
+
one Domain ID and one frame network, so a second watch naming other topics is refused at
|
|
382
|
+
init. Whether the Runtime publishes is a union instead: one declaration that wants to
|
|
383
|
+
publish is enough.
|
|
384
|
+
|
|
385
|
+
:param topic: The moving-transform topic, which a deployment may have remapped.
|
|
386
|
+
:param static_topic: The static-transform topic.
|
|
387
|
+
:param depth: How many messages each of the two subscriptions buffers.
|
|
388
|
+
:param broadcasting: Whether what opened this watch also publishes a frame of its own, which
|
|
389
|
+
advertises the two topics. It is asked here, at init, because :attr:`satisfied`
|
|
390
|
+
discounts our own publisher and the count must not change meaning at the first tick.
|
|
391
|
+
:return: A handle carrying its own lifetime. Close it when the Binding behind it is torn
|
|
392
|
+
down. The subscriptions and publishers close with the last watch.
|
|
393
|
+
:raises ROS2RuntimeError: When an earlier watch opened different sources.
|
|
394
|
+
"""
|
|
395
|
+
wanted = _Sources(topic=topic, static_topic=static_topic, depth=depth)
|
|
396
|
+
self._require_one_frame_network(wanted)
|
|
397
|
+
watch = TFWatch(facet=self, release=self._watches.remove)
|
|
398
|
+
self._watches.append(watch)
|
|
399
|
+
try:
|
|
400
|
+
await self._open(wanted)
|
|
401
|
+
if broadcasting:
|
|
402
|
+
await self._open_outward(wanted)
|
|
403
|
+
except BaseException:
|
|
404
|
+
self._watches.remove(watch)
|
|
405
|
+
await self.settle()
|
|
406
|
+
raise
|
|
407
|
+
return watch
|
|
408
|
+
|
|
409
|
+
async def broadcast(self, links: Iterable[FrameLink]) -> None:
|
|
410
|
+
"""Put every link on whichever of the two transform topics it belongs on.
|
|
411
|
+
|
|
412
|
+
One message per topic rather than per link, so a subscriber wakes once. An empty call sends
|
|
413
|
+
nothing. The stamp is one reading of the Runtime's clock for the whole call.
|
|
414
|
+
|
|
415
|
+
:param links: The links to publish. Each goes on ``/tf_static`` when it says it is static
|
|
416
|
+
and on ``/tf`` otherwise.
|
|
417
|
+
:raises ROS2RuntimeError: When no watch on this facet declared that it
|
|
418
|
+
broadcasts. This is checked before the payload, so an empty call is refused on the same
|
|
419
|
+
Machine a full one would be.
|
|
420
|
+
:raises PublishFailedError: When ``rclpy`` refused to send one of the two messages.
|
|
421
|
+
"""
|
|
422
|
+
# Checked before the empty case, so the refusal depends on the declaration and not on the payload.
|
|
423
|
+
self._require_advertised()
|
|
424
|
+
grouped: dict[bool, list[FrameLink]] = {}
|
|
425
|
+
for link in links:
|
|
426
|
+
grouped.setdefault(link.static, []).append(link)
|
|
427
|
+
seconds, nanoseconds = self._clock()
|
|
428
|
+
# Static first, matching the order the subscriptions open in.
|
|
429
|
+
for static in (True, False):
|
|
430
|
+
held = grouped.get(static)
|
|
431
|
+
if not held:
|
|
432
|
+
continue
|
|
433
|
+
publisher = self._outward[static]
|
|
434
|
+
await publisher.publish(_tf_message(publisher.message_type, held, seconds=seconds, nanoseconds=nanoseconds))
|
|
435
|
+
|
|
436
|
+
def _require_advertised(self) -> None:
|
|
437
|
+
"""Refuse a broadcast on a facet nothing declared an outward frame on.
|
|
438
|
+
|
|
439
|
+
:raises ROS2RuntimeError: Always, when the publishers are not open.
|
|
440
|
+
"""
|
|
441
|
+
if self._outward:
|
|
442
|
+
return
|
|
443
|
+
message = (
|
|
444
|
+
"something asked this Runtime to publish a transform and it declares no frame of its "
|
|
445
|
+
"own; the two transform publishers are opened by a watch that says it broadcasts, "
|
|
446
|
+
"because a publisher on /tf is what every TF Binding in the system reads as a "
|
|
447
|
+
"transform source being up"
|
|
448
|
+
)
|
|
449
|
+
raise ROS2RuntimeError(message)
|
|
450
|
+
|
|
451
|
+
def _require_one_frame_network(self, wanted: _Sources) -> None:
|
|
452
|
+
"""Refuse a second watch that reads TF from somewhere else.
|
|
453
|
+
|
|
454
|
+
:raises ROS2RuntimeError: When they do not agree.
|
|
455
|
+
"""
|
|
456
|
+
held = self._reading
|
|
457
|
+
if held is None or held == wanted:
|
|
458
|
+
return
|
|
459
|
+
message = (
|
|
460
|
+
f"TF is already being read from {held.topic!r} and {held.static_topic!r} at depth "
|
|
461
|
+
f"{held.depth}, and something asks for {wanted.topic!r} and {wanted.static_topic!r} "
|
|
462
|
+
f"at depth {wanted.depth}; one Runtime is one Domain ID and one frame network, so "
|
|
463
|
+
f"every declaration on this Machine reads it from the same two topics"
|
|
464
|
+
)
|
|
465
|
+
raise ROS2RuntimeError(message)
|
|
466
|
+
|
|
467
|
+
async def settle(self) -> None:
|
|
468
|
+
"""Close both subscriptions and both publishers if nothing is watching. Called by a closing watch."""
|
|
469
|
+
if self._watches or not (self._sources or self._outward):
|
|
470
|
+
return
|
|
471
|
+
self._reading = None
|
|
472
|
+
sources, self._sources = self._sources, []
|
|
473
|
+
for source in sources:
|
|
474
|
+
source.task.cancel()
|
|
475
|
+
for source in sources:
|
|
476
|
+
await asyncio.gather(source.task, return_exceptions=True)
|
|
477
|
+
await source.subscription.close()
|
|
478
|
+
outward, self._outward = self._outward, {}
|
|
479
|
+
for publisher in outward.values():
|
|
480
|
+
await publisher.close()
|
|
481
|
+
|
|
482
|
+
async def close(self) -> None:
|
|
483
|
+
"""End every watch and close every endpoint, which is what stopping the Runtime does."""
|
|
484
|
+
for watch in tuple(self._watches):
|
|
485
|
+
await watch.close()
|
|
486
|
+
await self.settle()
|
|
487
|
+
|
|
488
|
+
async def _open_outward(self, wanted: _Sources) -> None:
|
|
489
|
+
"""Advertise both transform topics, unless they are already advertised.
|
|
490
|
+
|
|
491
|
+
They are the pair the subscriptions use, so a remapped ``/tf`` is published where it is
|
|
492
|
+
read. What we publish comes back through our own subscription.
|
|
493
|
+
"""
|
|
494
|
+
if self._outward:
|
|
495
|
+
return
|
|
496
|
+
opened: dict[bool, Publisher] = {}
|
|
497
|
+
try:
|
|
498
|
+
for topic, static in ((wanted.static_topic, True), (wanted.topic, False)):
|
|
499
|
+
opened[static] = await self._advertise(
|
|
500
|
+
topic=topic,
|
|
501
|
+
message_type=TF_MESSAGE_TYPE,
|
|
502
|
+
depth=wanted.depth,
|
|
503
|
+
# Declared, and different per topic. See TF_STATIC_QOS.
|
|
504
|
+
qos=TF_STATIC_QOS if static else TF_QOS,
|
|
505
|
+
)
|
|
506
|
+
except BaseException:
|
|
507
|
+
for publisher in opened.values():
|
|
508
|
+
await publisher.close()
|
|
509
|
+
raise
|
|
510
|
+
self._outward = opened
|
|
511
|
+
|
|
512
|
+
async def _open(self, wanted: _Sources) -> None:
|
|
513
|
+
"""Open both transform topics, unless they are already open.
|
|
514
|
+
|
|
515
|
+
Static first, so a transient-local frame is in the record before the first moving one.
|
|
516
|
+
Neither subscription declares a profile, so both are discovered: the publishers decide what
|
|
517
|
+
is on somebody else's topic. That also recovers a transient-local ``/tf_static`` history
|
|
518
|
+
whether its publisher was up before us or comes up later.
|
|
519
|
+
"""
|
|
520
|
+
if self._sources:
|
|
521
|
+
return
|
|
522
|
+
opened: list[_Source] = []
|
|
523
|
+
try:
|
|
524
|
+
for topic, static in ((wanted.static_topic, True), (wanted.topic, False)):
|
|
525
|
+
subscription = await self._subscribe(
|
|
526
|
+
topic=topic,
|
|
527
|
+
message_type=TF_MESSAGE_TYPE,
|
|
528
|
+
depth=wanted.depth,
|
|
529
|
+
)
|
|
530
|
+
task = asyncio.create_task(self._follow(subscription, static=static), name=f"ros2-tf{topic}")
|
|
531
|
+
opened.append(_Source(subscription=subscription, task=task, static=static))
|
|
532
|
+
except BaseException:
|
|
533
|
+
for source in opened:
|
|
534
|
+
source.task.cancel()
|
|
535
|
+
await asyncio.gather(source.task, return_exceptions=True)
|
|
536
|
+
await source.subscription.close()
|
|
537
|
+
raise
|
|
538
|
+
self._sources = opened
|
|
539
|
+
self._reading = wanted
|
|
540
|
+
|
|
541
|
+
async def _follow(self, subscription: Subscription, *, static: bool) -> None:
|
|
542
|
+
"""Record every message of one transform topic, until the subscription closes."""
|
|
543
|
+
async for message in subscription:
|
|
544
|
+
try:
|
|
545
|
+
self._record(message, static=static)
|
|
546
|
+
except Exception:
|
|
547
|
+
# A malformed message must not end the stream.
|
|
548
|
+
_LOGGER.exception("Could not read a transform message", topic=subscription.topic)
|
|
549
|
+
|
|
550
|
+
def _record(self, message: Message, *, static: bool) -> None:
|
|
551
|
+
"""Put one message into the topology record and into the transform buffer.
|
|
552
|
+
|
|
553
|
+
The record feeds the partition and the buffer answers composed poses. Feeding the buffer may
|
|
554
|
+
raise on a transform tf2 refuses (an infinite value, a leading slash), which loses one
|
|
555
|
+
transform. Arrival is stamped per frame, because one message does not come from one
|
|
556
|
+
publisher (an arm driver dying silences only its frames), and from our own clock, not the
|
|
557
|
+
message's stamp, which a drifting clock or a replayed bag can put anywhere.
|
|
558
|
+
"""
|
|
559
|
+
arrived = self.now
|
|
560
|
+
if arrived and not self._clock_started:
|
|
561
|
+
# The clock has just started. Earlier stamps read zero and would all look as old as the clock's
|
|
562
|
+
# new reading, so every frame would go BadNoCommunication at once. Their true age is unknown, so
|
|
563
|
+
# now is the honest stamp.
|
|
564
|
+
self._seen = dict.fromkeys(self._seen, arrived)
|
|
565
|
+
self._clock_started = True
|
|
566
|
+
for link in links(message, static=static):
|
|
567
|
+
self._frames[link.child] = link
|
|
568
|
+
self._seen[link.child] = arrived
|
|
569
|
+
self._feed(message, static)
|
|
570
|
+
|
|
571
|
+
|
|
572
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
573
|
+
class _Sources:
|
|
574
|
+
"""Where one declaration reads TF from, which every declaration on a Machine must share."""
|
|
575
|
+
|
|
576
|
+
topic: str
|
|
577
|
+
static_topic: str
|
|
578
|
+
depth: int
|
|
579
|
+
|
|
580
|
+
|
|
581
|
+
@dataclass(frozen=True, slots=True, kw_only=True)
|
|
582
|
+
class _Source:
|
|
583
|
+
"""One of the two transform topics, live: its subscription and the task draining it.
|
|
584
|
+
|
|
585
|
+
:ivar static: Which of the two it is, so a matched count knows whether one of its publishers is
|
|
586
|
+
ours.
|
|
587
|
+
"""
|
|
588
|
+
|
|
589
|
+
subscription: Subscription
|
|
590
|
+
task: asyncio.Task[None]
|
|
591
|
+
static: bool
|
|
592
|
+
|
|
593
|
+
|
|
594
|
+
def _tf_message(
|
|
595
|
+
message_type: type[Message],
|
|
596
|
+
links: Sequence[FrameLink],
|
|
597
|
+
*,
|
|
598
|
+
seconds: int,
|
|
599
|
+
nanoseconds: int,
|
|
600
|
+
) -> Message:
|
|
601
|
+
"""Return the one ``TFMessage`` a list of links becomes.
|
|
602
|
+
|
|
603
|
+
Built rather than stamped: :func:`~opensmi.ros2.runtime.stamp.stamped` does not descend into
|
|
604
|
+
repeated message fields and every stamp in a ``TFMessage`` is inside one, so each stamp is
|
|
605
|
+
written as its element is constructed. Nested types are read off the dataclass, since nothing
|
|
606
|
+
here imports ``rclpy``.
|
|
607
|
+
"""
|
|
608
|
+
element_type = _element_type(message_type)
|
|
609
|
+
header_type = _nested_type(element_type, HEADER_FIELD)
|
|
610
|
+
time_type = _nested_type(header_type, STAMP_FIELD)
|
|
611
|
+
transform_type = _nested_type(element_type, TRANSFORM_FIELD)
|
|
612
|
+
vector_type = _nested_type(transform_type, TRANSLATION_FIELD)
|
|
613
|
+
rotation_type = _nested_type(transform_type, ROTATION_FIELD)
|
|
614
|
+
stamp = _built(time_type, **{SECONDS_FIELD: seconds, NANOSECONDS_FIELD: nanoseconds})
|
|
615
|
+
elements = [
|
|
616
|
+
_built(
|
|
617
|
+
element_type,
|
|
618
|
+
**{
|
|
619
|
+
HEADER_FIELD: _built(header_type, **{STAMP_FIELD: stamp, FRAME_FIELD: link.parent}),
|
|
620
|
+
CHILD_FRAME_FIELD: link.child,
|
|
621
|
+
TRANSFORM_FIELD: _built(
|
|
622
|
+
transform_type,
|
|
623
|
+
**{
|
|
624
|
+
TRANSLATION_FIELD: _built(
|
|
625
|
+
vector_type,
|
|
626
|
+
x=link.transform.translation[0],
|
|
627
|
+
y=link.transform.translation[1],
|
|
628
|
+
z=link.transform.translation[2],
|
|
629
|
+
),
|
|
630
|
+
ROTATION_FIELD: _built(
|
|
631
|
+
rotation_type,
|
|
632
|
+
x=link.transform.rotation[0],
|
|
633
|
+
y=link.transform.rotation[1],
|
|
634
|
+
z=link.transform.rotation[2],
|
|
635
|
+
w=link.transform.rotation[3],
|
|
636
|
+
),
|
|
637
|
+
},
|
|
638
|
+
),
|
|
639
|
+
},
|
|
640
|
+
)
|
|
641
|
+
for link in links
|
|
642
|
+
]
|
|
643
|
+
return _built(message_type, **{TRANSFORMS_FIELD: tuple(elements)})
|
|
644
|
+
|
|
645
|
+
|
|
646
|
+
def _built(message_type: type[Message], **values: Any) -> Message:
|
|
647
|
+
"""Return one message dataclass instance, built by field name.
|
|
648
|
+
|
|
649
|
+
A message dataclass has no statically known fields, so type checkers reject naming them at a
|
|
650
|
+
call site and accept a keyword splat.
|
|
651
|
+
"""
|
|
652
|
+
return message_type(**values)
|
|
653
|
+
|
|
654
|
+
|
|
655
|
+
def _element_type(message_type: type[Message]) -> type[Message]:
|
|
656
|
+
"""Return the type one element of a ``TFMessage``'s repeated field is.
|
|
657
|
+
|
|
658
|
+
:raises UnusableMessageTypeError: When the type advertised for the transform topics does not
|
|
659
|
+
repeat a message at all, which is a robot whose ``tf2_msgs`` is not ``tf2_msgs``.
|
|
660
|
+
"""
|
|
661
|
+
element = repeated_message(message_type, TRANSFORMS_FIELD)
|
|
662
|
+
if element is None:
|
|
663
|
+
message = (
|
|
664
|
+
f"{message_type.ros2.type_name} is what this Runtime advertised for the transform "
|
|
665
|
+
f"topics and it has no repeated {TRANSFORMS_FIELD!r} field of messages; a TFMessage "
|
|
666
|
+
f"is a list of stamped transforms and there is nothing to publish into"
|
|
667
|
+
)
|
|
668
|
+
raise UnusableMessageTypeError(message)
|
|
669
|
+
return element
|
|
670
|
+
|
|
671
|
+
|
|
672
|
+
def _nested_type(message_type: type[Message], field: str) -> type[Message]:
|
|
673
|
+
"""Return the message type one field of another holds.
|
|
674
|
+
|
|
675
|
+
:raises UnusableMessageTypeError: When the field is not a nested message, which is the same
|
|
676
|
+
condition as above one level down and says which level it was.
|
|
677
|
+
"""
|
|
678
|
+
nested = message_type.ros2_nested.get(field)
|
|
679
|
+
if nested is None:
|
|
680
|
+
message = (
|
|
681
|
+
f"{message_type.ros2.type_name} has no nested message field {field!r}, so a transform "
|
|
682
|
+
f"cannot be built out of it; the transform topics carry {TF_MESSAGE_TYPE}"
|
|
683
|
+
)
|
|
684
|
+
raise UnusableMessageTypeError(message)
|
|
685
|
+
return nested
|
|
686
|
+
|
|
687
|
+
|
|
688
|
+
def _named(value: object) -> str:
|
|
689
|
+
"""Return a frame id as the network keys it: text, without tf2's optional leading slash.
|
|
690
|
+
|
|
691
|
+
tf2 treats ``/base_link`` and ``base_link`` as one frame, so do we.
|
|
692
|
+
"""
|
|
693
|
+
return value.lstrip("/") if isinstance(value, str) else ""
|
|
694
|
+
|
|
695
|
+
|
|
696
|
+
def _transform_of(element: object) -> Transform | None:
|
|
697
|
+
"""Return one element's transform, or ``None`` when it does not carry the shape of one."""
|
|
698
|
+
transform = getattr(element, TRANSFORM_FIELD, None)
|
|
699
|
+
translation = _triple(getattr(transform, TRANSLATION_FIELD, None), ("x", "y", "z"))
|
|
700
|
+
rotation = _triple(getattr(transform, ROTATION_FIELD, None), ("x", "y", "z", "w"))
|
|
701
|
+
if translation is None or rotation is None:
|
|
702
|
+
return None
|
|
703
|
+
return Transform(translation=translation, rotation=rotation) # type: ignore[arg-type]
|
|
704
|
+
|
|
705
|
+
|
|
706
|
+
def _triple(held: object, names: tuple[str, ...]) -> tuple[float, ...] | None:
|
|
707
|
+
"""Return the named float fields of one nested message, or ``None`` if any is missing."""
|
|
708
|
+
values: list[float] = []
|
|
709
|
+
for name in names:
|
|
710
|
+
value = getattr(held, name, None)
|
|
711
|
+
if not isinstance(value, (int, float)) or isinstance(value, bool):
|
|
712
|
+
return None
|
|
713
|
+
values.append(float(value))
|
|
714
|
+
return tuple(values)
|