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.
Files changed (89) hide show
  1. opensmi/ros2/__init__.py +89 -0
  2. opensmi/ros2/__main__.py +11 -0
  3. opensmi/ros2/cli.py +143 -0
  4. opensmi/ros2/py.typed +0 -0
  5. opensmi/ros2/runtime/__init__.py +67 -0
  6. opensmi/ros2/runtime/_decode.py +101 -0
  7. opensmi/ros2/runtime/_encode.py +102 -0
  8. opensmi/ros2/runtime/_parser.py +147 -0
  9. opensmi/ros2/runtime/errors.py +141 -0
  10. opensmi/ros2/runtime/goal.py +232 -0
  11. opensmi/ros2/runtime/graph.py +288 -0
  12. opensmi/ros2/runtime/guard.py +213 -0
  13. opensmi/ros2/runtime/interface.py +140 -0
  14. opensmi/ros2/runtime/messages/__init__.py +503 -0
  15. opensmi/ros2/runtime/messages/industrial_msgs.py +62 -0
  16. opensmi/ros2/runtime/messages/industrial_msgs.pyi +159 -0
  17. opensmi/ros2/runtime/messages/motoros2_interfaces.py +79 -0
  18. opensmi/ros2/runtime/messages/motoros2_interfaces.pyi +307 -0
  19. opensmi/ros2/runtime/messages/neo_msgs2.py +55 -0
  20. opensmi/ros2/runtime/messages/neo_msgs2.pyi +138 -0
  21. opensmi/ros2/runtime/messages/neo_srvs2.py +72 -0
  22. opensmi/ros2/runtime/messages/neo_srvs2.pyi +224 -0
  23. opensmi/ros2/runtime/outcomes.py +103 -0
  24. opensmi/ros2/runtime/parameters.py +723 -0
  25. opensmi/ros2/runtime/publisher.py +125 -0
  26. opensmi/ros2/runtime/qos.py +394 -0
  27. opensmi/ros2/runtime/runtime.py +1171 -0
  28. opensmi/ros2/runtime/selection.py +150 -0
  29. opensmi/ros2/runtime/stamp.py +100 -0
  30. opensmi/ros2/runtime/subscription.py +354 -0
  31. opensmi/ros2/runtime/tf.py +714 -0
  32. opensmi/ros2/server/__init__.py +148 -0
  33. opensmi/ros2/server/access.py +109 -0
  34. opensmi/ros2/server/bindings/__init__.py +92 -0
  35. opensmi/ros2/server/bindings/_base.py +154 -0
  36. opensmi/ros2/server/bindings/action.py +133 -0
  37. opensmi/ros2/server/bindings/keyed.py +127 -0
  38. opensmi/ros2/server/bindings/lifecycle.py +131 -0
  39. opensmi/ros2/server/bindings/method.py +130 -0
  40. opensmi/ros2/server/bindings/monitoring.py +109 -0
  41. opensmi/ros2/server/bindings/parameter.py +133 -0
  42. opensmi/ros2/server/bindings/publisher.py +192 -0
  43. opensmi/ros2/server/bindings/registry.py +95 -0
  44. opensmi/ros2/server/bindings/tf.py +256 -0
  45. opensmi/ros2/server/browse_names.py +94 -0
  46. opensmi/ros2/server/component.py +334 -0
  47. opensmi/ros2/server/config.py +493 -0
  48. opensmi/ros2/server/containers.py +191 -0
  49. opensmi/ros2/server/drivers.py +421 -0
  50. opensmi/ros2/server/errors.py +159 -0
  51. opensmi/ros2/server/events.py +406 -0
  52. opensmi/ros2/server/introspection.py +850 -0
  53. opensmi/ros2/server/keyed.py +749 -0
  54. opensmi/ros2/server/lifecycle.py +238 -0
  55. opensmi/ros2/server/limits.py +66 -0
  56. opensmi/ros2/server/loader.py +977 -0
  57. opensmi/ros2/server/machine.py +225 -0
  58. opensmi/ros2/server/method.py +307 -0
  59. opensmi/ros2/server/mirror.py +582 -0
  60. opensmi/ros2/server/mutation.py +992 -0
  61. opensmi/ros2/server/nodesets/Opc.Ua.SmartFactory.ROS2.NodeSet2.xml +85 -0
  62. opensmi/ros2/server/nodesets/__init__.py +18 -0
  63. opensmi/ros2/server/nodesets/smart_factory_ros2.py +26 -0
  64. opensmi/ros2/server/parameters.py +704 -0
  65. opensmi/ros2/server/projection.py +1176 -0
  66. opensmi/ros2/server/projections/__init__.py +41 -0
  67. opensmi/ros2/server/projections/scope.py +222 -0
  68. opensmi/ros2/server/provenance.py +121 -0
  69. opensmi/ros2/server/publisher.py +781 -0
  70. opensmi/ros2/server/recipes.py +514 -0
  71. opensmi/ros2/server/required.py +633 -0
  72. opensmi/ros2/server/skill.py +363 -0
  73. opensmi/ros2/server/spatial.py +283 -0
  74. opensmi/ros2/server/startup.py +626 -0
  75. opensmi/ros2/server/status.py +107 -0
  76. opensmi/ros2/server/structures.py +135 -0
  77. opensmi/ros2/server/transforms/__init__.py +39 -0
  78. opensmi/ros2/server/transforms/conversions.py +58 -0
  79. opensmi/ros2/server/transforms/filing.py +340 -0
  80. opensmi/ros2/server/transforms/mirror.py +462 -0
  81. opensmi/ros2/server/transforms/outward.py +180 -0
  82. opensmi/ros2/server/transforms/records.py +195 -0
  83. opensmi/ros2/server/transforms/spatial.py +154 -0
  84. opensmi_ros2-0.1.0.dist-info/METADATA +183 -0
  85. opensmi_ros2-0.1.0.dist-info/RECORD +89 -0
  86. opensmi_ros2-0.1.0.dist-info/WHEEL +5 -0
  87. opensmi_ros2-0.1.0.dist-info/entry_points.txt +2 -0
  88. opensmi_ros2-0.1.0.dist-info/licenses/LICENSES/MIT.txt +18 -0
  89. 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)