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,462 @@
1
+ # SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
2
+ #
3
+ # SPDX-License-Identifier: MIT
4
+
5
+ """Inward mirroring: the poll that renders the frame network as Locations."""
6
+
7
+ from __future__ import annotations
8
+
9
+ import asyncio
10
+ from collections.abc import AsyncGenerator
11
+ from typing import TYPE_CHECKING, Any, Final
12
+
13
+ from asyncua import ua
14
+ from opensmi.core.lifecycle_mixin import lifecycle
15
+ from opensmi.server.base_machinery_item import BaseMachineryItem
16
+ from opensmi.server.spatial_object import Orientation, Pose, Position
17
+
18
+ from opensmi.ros2.runtime.tf import FrameLink
19
+ from opensmi.ros2.server.errors import (
20
+ FrameDirectionError,
21
+ )
22
+ from opensmi.ros2.server.events import Severity
23
+ from opensmi.ros2.server.mutation import ended
24
+ from opensmi.ros2.server.spatial import (
25
+ angles,
26
+ attach_points,
27
+ excluded,
28
+ one_direction_per_frame,
29
+ owners,
30
+ parents_of,
31
+ tree_roots,
32
+ )
33
+ from opensmi.ros2.server.status import GOOD, status_value
34
+ from opensmi.ros2.server.transforms.conversions import _LEVEL, _ZERO, DEGREES, METRES
35
+ from opensmi.ros2.server.transforms.outward import _OutwardPublishing
36
+ from opensmi.ros2.server.transforms.records import (
37
+ _Bearers,
38
+ _Frame,
39
+ _Owner,
40
+ _Quiet,
41
+ )
42
+
43
+ if TYPE_CHECKING:
44
+ from collections.abc import Iterator, Mapping, Sequence
45
+
46
+ from asyncua.common.node import Node
47
+
48
+
49
+ SEPARATE_FRAME_TREES: Final = "SEPARATE_FRAME_TREES"
50
+ """The BindingEvent code more than one root in the rendered frame network carries.
51
+
52
+ A well-formed TF tree has one root, which OPC 10000-210 calls the World Frame. With several,
53
+ unrelated coordinate systems are presented as one.
54
+ """
55
+
56
+
57
+ class ROS2TransformMirrorMixin(_OutwardPublishing):
58
+ """Keeps every SpatialObject on one Machine in step with the TF tree, on a timer.
59
+
60
+ Machine only: a frame belongs to the nearest declared head above it, and the heads are spread
61
+ over the Machine and its Components. The period is declared on the Machine for the same reason
62
+ (:attr:`~opensmi.ros2.server.config.TransformConfig.interval`). How many Locations one TF
63
+ Binding may gain is :attr:`~opensmi.ros2.server.mutation.ROS2MutationMixin.mutation_node_cap` on
64
+ the declaring item; a robot with more frames says so once.
65
+ """
66
+
67
+ def __init__(self, *args: Any, **kwargs: Any) -> None:
68
+ """Start out with nothing rendered, and pass every argument to opensmi."""
69
+ super().__init__(*args, **kwargs)
70
+ self._transform_interval = 0.0
71
+ self._transform_task: asyncio.Task[None] | None = None
72
+ self._transform_polls = 0
73
+ self._transform_outward: frozenset[str] = frozenset()
74
+ self._transform_roots: tuple[str, ...] = ()
75
+ self._transform_quiet_after = 0.0
76
+
77
+ @property
78
+ def ros2_transform_polls(self) -> int:
79
+ """Return how often the frame network has been read, which a Machine with no TF leaves at 0."""
80
+ return self._transform_polls
81
+
82
+ @property
83
+ def ros2_frame_nodes(self) -> Mapping[str, str]:
84
+ """Return the frame-to-node index: every rendered frame and the node holding it, as NodeId text."""
85
+ return {frame: node.to_string() for frame, node in self._transform_index.items()}
86
+
87
+ @lifecycle(after=BaseMachineryItem.lifecycle_machinery_item)
88
+ async def lifecycle_ros2_transform_mirror(self) -> AsyncGenerator[None]:
89
+ """Start the poll once the whole Machine is built, and stop it before teardown.
90
+
91
+ It starts after the Machine's ``_init`` because the Components' objects must exist first,
92
+ and stops before ``_shutdown`` so no tick writes into a Component that is going away.
93
+ """
94
+ await self._init_transform_mirror()
95
+ yield
96
+ await self._stop_transform_mirror()
97
+
98
+ async def _init_transform_mirror(self) -> None:
99
+ """Collect every SpatialObject on this Machine and start the poll, if there are any.
100
+
101
+ Nothing is created for frames the robot publishes and nothing is asked of it: the first tick
102
+ is one interval away. Declared outward frames are created here, so a client may write them
103
+ before the robot is on. The cross-object direction check runs first.
104
+
105
+ :raises FrameDirectionError: When two declarations on this Machine claim one frame.
106
+ """
107
+ objects = self._declared_spatial_objects()
108
+ if not objects:
109
+ return
110
+ listed: Any = getattr(self, "spatial_object_list", None)
111
+ self._transform_world = listed.world_frame.ua_node.nodeid
112
+ self._transform_objects = objects
113
+ self._require_one_direction_per_frame(objects)
114
+ self._transform_outward = frozenset(frame for owner in objects for frame in owner.binding.published_frames)
115
+ for owner in objects:
116
+ head = owner.binding.head
117
+ self._transform_index[head] = owner.spatial.position_frame.ua_node.nodeid
118
+ # The head's Location is the mandatory PositionFrame, which opensmi built.
119
+ owner.frames[head] = _Frame(
120
+ frame=head,
121
+ browse=head,
122
+ location=owner.spatial.position_frame,
123
+ folder=None,
124
+ )
125
+ for owner in objects:
126
+ for declared in owner.binding.publishes:
127
+ await self._open_published(owner, declared)
128
+ # One poll, one period: the Machine's.
129
+ self._transform_interval = self.ros2_transform_settings.interval
130
+ self._transform_quiet_after = self.ros2_transform_settings.quiet_after
131
+ self._transform_task = asyncio.create_task(self._poll_transforms(), name="ros2-tf-poll")
132
+
133
+ def _require_one_direction_per_frame(self, objects: Sequence[_Owner]) -> None:
134
+ """Refuse a Machine on which two declarations claim one frame.
135
+
136
+ This is the cross-object half of the inward/outward rule, which no single declaration can
137
+ see. It runs before anything is created.
138
+
139
+ :raises FrameDirectionError: When one frame is claimed twice.
140
+ """
141
+ clash = one_direction_per_frame(
142
+ heads={owner.binding.name: owner.binding.head for owner in objects},
143
+ published={owner.binding.name: owner.binding.published_frames for owner in objects},
144
+ )
145
+ if clash is None:
146
+ return
147
+ if clash.inward:
148
+ against = (
149
+ f"the TF Binding {clash.against!r} declares it as its head frame, which is read "
150
+ f"*inward* from the tree -- so the pose a client wrote would be written over by "
151
+ f"the robot's answer on the next tick, for ever"
152
+ )
153
+ else:
154
+ against = (
155
+ f"the TF Binding {clash.against!r} publishes it too -- and a frame has one parent "
156
+ f"and one publisher, so two declarations asserting it would put two transforms on "
157
+ f"the topic every tick and leave tf2 keeping whichever arrived last"
158
+ )
159
+ message = (
160
+ f"the TF Binding {clash.outward!r} publishes the frame {clash.frame!r} and {against}. "
161
+ f"A frame is inward or outward and never both, which is what makes an echo loop "
162
+ f"impossible rather than something to suppress"
163
+ )
164
+ raise FrameDirectionError(message)
165
+
166
+ async def _stop_transform_mirror(self) -> None:
167
+ """Stop the poll. Idempotent, and it leaves the watches to the items that opened them."""
168
+ await ended(self._transform_task)
169
+ self._transform_task = None
170
+ self._transform_objects = ()
171
+
172
+ def _declared_spatial_objects(self) -> tuple[_Owner, ...]:
173
+ """Return this Machine's SpatialObject and every Component's, in browse order."""
174
+ found: list[_Owner] = []
175
+ for item in _machinery_items(self):
176
+ binding = getattr(item, "ros2_tf_binding", None)
177
+ spatial = getattr(item, "ros2_spatial_object", None)
178
+ if binding is None or spatial is None:
179
+ continue
180
+ found.append(_Owner(item=item, binding=binding, spatial=spatial))
181
+ return tuple(found)
182
+
183
+ async def _poll_transforms(self) -> None:
184
+ """Read the frame network on the interval and render it, until cancelled."""
185
+ while True:
186
+ await asyncio.sleep(self._transform_interval)
187
+ try:
188
+ await self.refresh_transforms()
189
+ except Exception:
190
+ # A runtime condition is never fatal; a tree that failed once is likely fine on the
191
+ # next tick.
192
+ self.logger.exception("Could not render the TF tree; the poll continues")
193
+
194
+ async def refresh_transforms(self) -> None:
195
+ """Read the frame network once and bring every SpatialObject into step with it.
196
+
197
+ Public so that tests can drive the inward path without waiting for the timer.
198
+ """
199
+ self._transform_polls += 1
200
+ # A snapshot, because the facet's record is live. Outward frames are dropped from it: our own
201
+ # broadcast comes back through our subscription and cannot be told from the robot's, so an outward
202
+ # pose is only ever read from its Location.
203
+ frames = {
204
+ frame: link
205
+ for frame, link in self._ros2_runtime_for_tf().tf.frames.items()
206
+ if frame not in self._transform_outward
207
+ }
208
+ # Declared outward links join the tree, so the partition, attach points, folders and Bases treat them
209
+ # like published ones.
210
+ parents = dict(parents_of(frames))
211
+ for owner in self._transform_objects:
212
+ for declared in owner.binding.publishes:
213
+ parents[declared.frame] = declared.base
214
+ roots = tree_roots(parents)
215
+ await self._report_separate_trees(roots)
216
+ facet = self._ros2_runtime_for_tf().tf
217
+ # One reading of the clock and of the presence per tick, so no two Locations disagree.
218
+ quiet = _Quiet(
219
+ present=facet.satisfied,
220
+ seen=facet.seen,
221
+ now=facet.now,
222
+ within=self._transform_quiet_after,
223
+ roots=frozenset(roots),
224
+ )
225
+ heads = {owner.binding.head for owner in self._transform_objects}
226
+ held = owners(parents=parents, heads=heads)
227
+ attach = attach_points(parents=parents, heads=heads)
228
+ for owner in self._transform_objects:
229
+ owner.wanted = {
230
+ frame
231
+ for frame, head in held.items()
232
+ if head == owner.binding.head and not excluded(frame, owner.binding.exclude)
233
+ }
234
+ # Always: the head is the mandatory PositionFrame.
235
+ owner.wanted.add(owner.binding.head)
236
+ await self._create_new_locations(owner, parents=parents, attach=attach)
237
+ for owner in self._transform_objects:
238
+ await self._write_locations(owner, frames=frames, parents=parents, quiet=quiet)
239
+ await self._write_alternatives(owner, frames=frames, quiet=quiet)
240
+ await self._rebase_published(owner, parents=parents)
241
+ await self._broadcast_published()
242
+
243
+ async def _write_locations(
244
+ self,
245
+ owner: _Owner,
246
+ *,
247
+ frames: Mapping[str, FrameLink],
248
+ parents: Mapping[str, str],
249
+ quiet: _Quiet,
250
+ ) -> None:
251
+ """Write every Location of one object: its pose, and the Base its TF parent implies.
252
+
253
+ This runs after all new Locations exist, so a Base never points at the World Frame for one
254
+ tick and at a Location the next.
255
+ """
256
+ for frame, held in sorted(owner.frames.items()):
257
+ link = frames.get(frame)
258
+ await self._write_one(
259
+ held,
260
+ link,
261
+ base=self._base_node(frame, parents=parents),
262
+ status=quiet.status_of(frame, link),
263
+ )
264
+
265
+ async def _write_one(
266
+ self,
267
+ held: _Frame,
268
+ link: FrameLink | None,
269
+ *,
270
+ base: ua.NodeId,
271
+ status: ua.StatusCode,
272
+ ) -> None:
273
+ """Write one Location from the transform its TF parent published, and say how it is.
274
+
275
+ A frame nothing has published yet still gets its Base and status.
276
+ """
277
+ pose = None if link is None else (link.transform.translation, link.transform.rotation)
278
+ await self._write_pose(held, pose, base=base, status=status)
279
+ if link is not None and link.static and not held.constant:
280
+ # Staticness is a fact about the message, and a head frame's Location exists before any message.
281
+ await self._mark_constant(held.node)
282
+ held.constant = True
283
+
284
+ async def _write_pose(
285
+ self,
286
+ held: _Frame,
287
+ pose: tuple[tuple[float, float, float], tuple[float, float, float, float]] | None,
288
+ *,
289
+ base: ua.NodeId,
290
+ status: ua.StatusCode,
291
+ ) -> None:
292
+ """Write one Location's pose, status and Base, and only what changed.
293
+
294
+ Writing an unchanged value still wakes every MonitoredItem on the node. The status counts as
295
+ a change: a bad status nulls its value (OPC UA Part 4 7.7.1), so a frame that returns at the
296
+ pose it left must still be written. Used for both a frame's Location and an alternative's,
297
+ which differ only in where the pose came from.
298
+ """
299
+ if pose is not None:
300
+ translation, turned = pose
301
+ position = Position(*translation, unit=METRES)
302
+ orientation = Orientation(*angles(turned), unit=DEGREES)
303
+ else:
304
+ # No pose yet. The status still needs a value to go with (see status.py), so keep what the node
305
+ # reads.
306
+ position = held.position or _ZERO
307
+ orientation = held.orientation or _LEVEL
308
+ if position != held.position or orientation != held.orientation or status != held.status:
309
+ await self._write_reading(held, position, orientation, status)
310
+ if base != held.base:
311
+ await held.location.write_base(base)
312
+ held.base = base
313
+
314
+ async def _write_reading(
315
+ self,
316
+ held: _Frame,
317
+ position: Position,
318
+ orientation: Orientation,
319
+ status: ua.StatusCode,
320
+ ) -> None:
321
+ """Put one pose on a Location's nine value-bearing nodes, with its status.
322
+
323
+ A Good pose goes through opensmi's ``write_pose``. opensmi cannot write a status code, so a
324
+ bad status is written by hand, always with the value, because a status alone leaves the
325
+ variant null (see :mod:`~opensmi.ros2.server.status`).
326
+ """
327
+ if status == GOOD:
328
+ await held.location.write_pose(Pose(position, orientation))
329
+ else:
330
+ await self._write_faulted(held, position, orientation, status)
331
+ held.position = position
332
+ held.orientation = orientation
333
+ held.status = status
334
+
335
+ async def _write_faulted(
336
+ self,
337
+ held: _Frame,
338
+ position: Position,
339
+ orientation: Orientation,
340
+ status: ua.StatusCode,
341
+ ) -> None:
342
+ """Write one bad reading onto the Location, its pose and its six scalars.
343
+
344
+ The scalars too, because clients subscribe to ``Position/X``. Not the ``Base``, which stays
345
+ true while the frame is unreachable. The Location is written first; opensmi's child sync
346
+ ignores a null value, so the children keep what is written to them next.
347
+ """
348
+ if held.bearers is None:
349
+ held.bearers = await self._pose_nodes(held.node)
350
+ bearers = held.bearers
351
+ cartesian = ua.ThreeDCartesianCoordinates(
352
+ X=ua.Double(position.x), Y=ua.Double(position.y), Z=ua.Double(position.z)
353
+ )
354
+ turned = ua.ThreeDOrientation(
355
+ A=ua.Double(orientation.a), B=ua.Double(orientation.b), C=ua.Double(orientation.c)
356
+ )
357
+ extension = ua.VariantType.ExtensionObject
358
+ await bearers.frame.write_value(
359
+ status_value(ua.ThreeDFrame(CartesianCoordinates=cartesian, Orientation=turned), extension, status)
360
+ )
361
+ await asyncio.gather(
362
+ bearers.position.write_value(status_value(cartesian, extension, status)),
363
+ bearers.orientation.write_value(status_value(turned, extension, status)),
364
+ *[
365
+ node.write_value(status_value(ua.Double(value), ua.VariantType.Double, status))
366
+ for node, value in (
367
+ (bearers.x, position.x),
368
+ (bearers.y, position.y),
369
+ (bearers.z, position.z),
370
+ (bearers.a, orientation.a),
371
+ (bearers.b, orientation.b),
372
+ (bearers.c, orientation.c),
373
+ )
374
+ ],
375
+ )
376
+
377
+ async def _pose_nodes(self, node: Node) -> _Bearers:
378
+ """Browse the nine value-bearing nodes of one Location, once, on its first bad write.
379
+
380
+ The result is safe to keep because nodes are never moved.
381
+ """
382
+ ns = self._rsl_namespace
383
+ position = await node.get_child(f"{ns}:Position")
384
+ orientation = await node.get_child(f"{ns}:Orientation")
385
+ # Named, so that a wrong arity is caught by the type checker.
386
+ return _Bearers(
387
+ frame=node,
388
+ position=position,
389
+ x=await position.get_child("X"),
390
+ y=await position.get_child("Y"),
391
+ z=await position.get_child("Z"),
392
+ orientation=orientation,
393
+ a=await orientation.get_child(f"{ns}:A"),
394
+ b=await orientation.get_child(f"{ns}:B"),
395
+ c=await orientation.get_child(f"{ns}:C"),
396
+ )
397
+
398
+ async def _write_alternatives(self, owner: _Owner, *, frames: Mapping[str, FrameLink], quiet: _Quiet) -> None:
399
+ """Compose and write every alternative this object declared, creating new ones.
400
+
401
+ This is the only place the transform buffer is asked anything. The lookup is synchronous,
402
+ and a chain it cannot complete is skipped. An alternative carries the status of the frame it
403
+ views, so two nodes never disagree about a quiet frame.
404
+ """
405
+ facet = self._ros2_runtime_for_tf().tf
406
+ for base, frame in owner.binding.alternatives:
407
+ composed = facet.lookup(target=base, source=frame)
408
+ if composed is None:
409
+ continue
410
+ node = self._alternative_base(base, frames)
411
+ if node is None:
412
+ await self._report_absent_base(owner, base, frame)
413
+ continue
414
+ held = owner.alternatives.get((base, frame))
415
+ if held is None:
416
+ held = await self._open_alternative(owner, base, frame, node)
417
+ if held is None:
418
+ continue
419
+ await self._write_pose(
420
+ held,
421
+ (composed.translation, composed.rotation),
422
+ base=node,
423
+ status=quiet.status_of(frame, frames.get(frame)),
424
+ )
425
+
426
+ async def _report_separate_trees(self, roots: tuple[str, ...]) -> None:
427
+ """Say, as a BindingEvent, that more than one coordinate system is being presented as one.
428
+
429
+ Once per distinct set of roots, and there is no clearing event. Nothing is said while no
430
+ robot is publishing: before that, a Machine that publishes two frames under different bases
431
+ has two roots by construction. Reported on the Machine's own TF Binding.
432
+ """
433
+ if not self._ros2_runtime_for_tf().tf.satisfied:
434
+ # Re-evaluated, so the condition is reported the first time it is seen against a robot that is on.
435
+ self._transform_roots = ()
436
+ return
437
+ if roots == self._transform_roots:
438
+ return
439
+ self._transform_roots = roots
440
+ binding = self.ros2_tf_binding
441
+ if binding is None or len(roots) < 2:
442
+ return
443
+ listed = ", ".join(repr(root) for root in roots)
444
+ self.logger.warning("The frame network has more than one root", roots=roots)
445
+ await self.emit_binding_event(
446
+ binding=binding.name,
447
+ path=self.ros2_transform_settings.path,
448
+ code=SEPARATE_FRAME_TREES,
449
+ severity=Severity.WARNING,
450
+ text=(
451
+ f"the frame network has {len(roots)} roots ({listed}); a well-formed TF tree has "
452
+ f"one, and every Location whose Base walk leaves the rendered tree lands on the "
453
+ f"same WorldFrame -- so these unrelated coordinate systems are presented as one"
454
+ ),
455
+ )
456
+
457
+
458
+ def _machinery_items(machine: Any) -> Iterator[Any]:
459
+ """Yield a Machine and every Component under it, however deeply nested."""
460
+ yield machine
461
+ for component in getattr(machine, "components", ()):
462
+ yield from _machinery_items(component)
@@ -0,0 +1,180 @@
1
+ # SPDX-FileCopyrightText: 2026 Technologie-Initiative SmartFactory KL e.V.
2
+ #
3
+ # SPDX-License-Identifier: MIT
4
+
5
+ """Outward publishing: declared frames a client may write, and their broadcast."""
6
+
7
+ from __future__ import annotations
8
+
9
+ import asyncio
10
+ from typing import TYPE_CHECKING, Any
11
+
12
+ from opensmi.server.spatial_object import Orientation, Pose, Position
13
+
14
+ from opensmi.ros2.runtime.errors import PublishFailedError
15
+ from opensmi.ros2.runtime.tf import FrameLink, Transform
16
+ from opensmi.ros2.server.bindings import PublishedFrame
17
+ from opensmi.ros2.server.config import ROS2Config
18
+ from opensmi.ros2.server.events import Severity
19
+ from opensmi.ros2.server.keyed import member_browse_name
20
+ from opensmi.ros2.server.mutation import _write_display_name
21
+ from opensmi.ros2.server.publisher import PUBLISH_FAILED
22
+ from opensmi.ros2.server.spatial import (
23
+ ATTACH_POINTS,
24
+ )
25
+ from opensmi.ros2.server.transforms.conversions import DEGREES, METRES, _as_transform
26
+ from opensmi.ros2.server.transforms.filing import _FrameFiling
27
+ from opensmi.ros2.server.transforms.records import (
28
+ _Observed,
29
+ _Owner,
30
+ _Published,
31
+ )
32
+
33
+ if TYPE_CHECKING:
34
+ from collections.abc import Awaitable, Callable, Mapping, Sequence
35
+
36
+
37
+ class _OutwardPublishing(_FrameFiling):
38
+ """Puts the poses a client writes into declared Locations on the TF topics."""
39
+
40
+ def __init__(self, *args: Any, **kwargs: Any) -> None:
41
+ """Start out with nothing refused, and pass every argument to opensmi."""
42
+ super().__init__(*args, **kwargs)
43
+ self._broadcast_refused = False
44
+ self._broadcast_lock = asyncio.Lock()
45
+
46
+ async def _open_published(self, owner: _Owner, declared: PublishedFrame) -> None:
47
+ """Create one declared outward frame's Location, writable, in ``AttachPoints``.
48
+
49
+ It is created at init and outside the chokepoint because it is declared. Access level and
50
+ write check are set here, since a node cannot be re-shaped later. A client needs the
51
+ Machine's Skill level (:attr:`~opensmi.ros2.server.config.ROS2Config.skill_access_level`),
52
+ as for a publishing Skill, and the Lock. ``Base`` stays read-only. It starts at the World
53
+ Frame and is re-resolved every tick, because the declared base frame may not be published
54
+ yet.
55
+ """
56
+ browse = member_browse_name((declared.frame,))
57
+ world = self._require_world()
58
+ location = _Observed(
59
+ name=browse,
60
+ initial_pose=Pose(
61
+ Position(*declared.position, unit=METRES),
62
+ Orientation(*declared.orientation, unit=DEGREES),
63
+ ),
64
+ initial_base=world,
65
+ # Opt-in writable pose; writes reach this module once stored.
66
+ minimum_access_level=(getattr(self, "ros2_config", None) or ROS2Config()).skill_access_level,
67
+ written=self._pose_written(owner, declared.frame),
68
+ )
69
+ await self._file(owner, ATTACH_POINTS, location)
70
+ owner.folders.add(ATTACH_POINTS)
71
+ await _write_display_name(location.ua_node, declared.frame)
72
+ owner.claimed[browse] = declared.frame
73
+ owner.published[declared.frame] = _Published(declared=declared, location=location, base=world)
74
+ self._transform_index[declared.frame] = location.ua_node.nodeid
75
+
76
+ def _pose_written(self, owner: _Owner, frame: str) -> Callable[[Pose], Awaitable[None]]:
77
+ """Return the callback one outward Location is created with. opensmi awaits it with the stored pose."""
78
+
79
+ async def written(pose: Pose) -> None:
80
+ await self._broadcast_written(owner, frame, pose)
81
+
82
+ return written
83
+
84
+ async def _broadcast_written(self, owner: _Owner, frame: str, pose: Pose) -> None:
85
+ """Put the pose a client wrote on the wire.
86
+
87
+ The write is never refused for the broadcast: it is already stored, and a failed broadcast
88
+ is a ``PUBLISH_FAILED`` BindingEvent that the next tick retries.
89
+ """
90
+ held = owner.published.get(frame)
91
+ if held is None:
92
+ return
93
+ await self._broadcast([(held, _as_transform(pose.position, pose.orientation))])
94
+
95
+ async def _rebase_published(self, owner: _Owner, *, parents: Mapping[str, str]) -> None:
96
+ """Re-resolve every outward Location's ``Base``, the only part of it this module writes."""
97
+ for held in owner.published.values():
98
+ base = self._base_node(held.declared.frame, parents=parents)
99
+ if base == held.base:
100
+ continue
101
+ await held.location.write_base(base)
102
+ held.base = base
103
+
104
+ async def _broadcast_published(self) -> None:
105
+ """Re-assert what every outward Location holds, in one message per topic.
106
+
107
+ A moving frame goes out every tick, because a pose published once ages out of listeners'
108
+ buffers. A static frame goes out only when it differs from what last went out, which also
109
+ retries a pose the middleware refused. That is why
110
+ :meth:`~opensmi.ros2.runtime.tf.TFFacet.broadcast` owns its publisher instead of using
111
+ ``tf2_ros.StaticTransformBroadcaster``. The pose is read back off the Location because it is
112
+ the client's.
113
+ """
114
+ staged: list[tuple[_Published, Transform]] = []
115
+ for owner in self._transform_objects:
116
+ for held in owner.published.values():
117
+ pose = await held.location.read_pose()
118
+ transform = _as_transform(pose.position, pose.orientation)
119
+ if held.declared.static and transform == held.sent:
120
+ continue
121
+ staged.append((held, transform))
122
+ await self._broadcast(staged)
123
+
124
+ async def _broadcast(self, staged: Sequence[tuple[_Published, Transform]]) -> None:
125
+ """Put outward poses on the wire, and record what went out only once it has.
126
+
127
+ A refusal is reported as an event and never raised. An outward pose carries no status code,
128
+ because the quiet conditions of :mod:`~opensmi.ros2.server.status` are facts about a robot
129
+ publishing to us. Serialised, because a client's write and the tick may both broadcast.
130
+ """
131
+ if not staged:
132
+ return
133
+ sending = [
134
+ FrameLink(
135
+ child=held.declared.frame,
136
+ parent=held.declared.base,
137
+ transform=transform,
138
+ static=held.declared.static,
139
+ )
140
+ for held, transform in staged
141
+ ]
142
+ async with self._broadcast_lock:
143
+ try:
144
+ await self._ros2_runtime_for_tf().tf.broadcast(sending)
145
+ except PublishFailedError as error:
146
+ await self._report_broadcast_refused(sending, error)
147
+ return
148
+ # Recorded after the wire. Recorded before, a refused static frame would compare equal on every
149
+ # later tick and never be retried.
150
+ for held, transform in staged:
151
+ held.sent = transform
152
+ self._broadcast_refused = False
153
+
154
+ async def _report_broadcast_refused(self, sending: Sequence[FrameLink], error: Exception) -> None:
155
+ """Say, as a BindingEvent, that the poses a client wrote did not reach ROS2.
156
+
157
+ Once per spell of failure; the streak clears on the first broadcast that goes out. It is
158
+ reported on the Machine's own TF Binding, because one broadcast carries every object's
159
+ frames. It uses :data:`~opensmi.ros2.server.publisher.PUBLISH_FAILED`, the code the
160
+ publishing Skill reports for the same fact.
161
+ """
162
+ binding = self.ros2_tf_binding
163
+ if binding is None: # pragma: no cover -- nothing broadcasts without a declaration
164
+ return
165
+ if self._broadcast_refused:
166
+ return
167
+ # After both guards, so the latch is only set by a failure that was reported.
168
+ self._broadcast_refused = True
169
+ frames = sorted(link.child for link in sending)
170
+ self.logger.error("A transform broadcast did not happen", binding=binding.name, frames=frames)
171
+ await self.emit_binding_event(
172
+ binding=binding.name,
173
+ path=self.ros2_transform_settings.path,
174
+ code=PUBLISH_FAILED,
175
+ severity=Severity.ERROR,
176
+ text=(
177
+ f"the poses written into {', '.join(frames)} did not reach ROS2: {error}; the Locations hold "
178
+ f"what a client wrote and nothing on the robot has been told about it"
179
+ ),
180
+ )